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ABSTRACT 


The ability of an Autonomous Underwater Vehicle (AUV) to dynamically plan safe routes and 
maneuvers in dangerous environments is directly relevant for the future of the use of AUVs 
in the exploration and exploitation of the underwater environment, specifically the littorals 
and inland waters. This thesis builds upon the existing body of knowledge of the REMUS 
AUV dynamics and kinematics and develops a control scheme for a real-time optimized vehicle 
trajectory that will permit continuous and autonomous collection and exploitation of external 
sensor data, which will facilitate full 360-degree, 2-dimensional mapping of the underwater en- 
vironment surrounding the vehicle while preventing the vehicle from coming into contact with 
mapped objects in the water. The developed control schema will seek to generate a trajectory in 
real-time that optimizes a key parameter of interest, the Information Gain, while minimizing a 


specified cost function of constraints, such as kinematic limits and obstacle avoidance criteria. 
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CHAPTER 1: 


Introduction 





The ocean is a vast and ever-changing environment. It presents significant challenges for those 
that seek to explore and exploit it. Much of it is unknown and unexplored, and even much of 
the known areas can be inhospitable and downright hostile to human intrusion. It has been said 
that we know more about outer space than our own oceans [1]; yet it remains one of mankind’s 
most vital and heavily exploited environments [2]. Ships have plied the seas for exploration, 
merchant trade, and military operations for millennia, but until the last century, what lie below 
the surface of the sea remained in the realm of myth and fantasy. While merchant mariners 
remain largely content to safely ply the navigable waters on the surface with little concern 
as to what lies below, the undersea environment is of great interest to scientists, engineers, 
and military planners and operators. To that end, great strides have been made in exploring 
this undersea world, from the deepest abysses to the shallow littorals and inland waterways 
connected therewith. As evidenced by recent funding, research and development priorities, the 


Navy has significant interest in exploring these shallow water environments [3]. 


1.1 Motivation 

Dominance of the shallow waters of the littorals and inland waters is a vital concern for Mar- 
itime Component Commanders. Amphibious operations, Naval Surface Gunfire Support mis- 
sions, Special Operations, submarine strike packages, near-shore and inshore reconnaissance, 
mine countermeasures, riverine combat, and waterway defense are just a few of the Naval op- 
erations that exploit this waterspace. To enable such dominance, the Commander must have 
the tools at his disposal to safely and completely explore unknown areas, investigate and map 
underwater features, and locate and counter submerged mines. Remotely Operated Vehicles 
(ROV’s) and Autonomous Underwater Vehicles (AUVs) are being used extensively in fields of 
underwater cartography, exploration, salvage, and oceanographic research [4]. In recent years, 
the Navy has recognized their extreme utility in meeting the aforementioned needs in the context 
of military planning and operations. Just as Unmanned Aerial Vehicles have seen widespread 
use in similar roles by Air and Ground Commanders, AUV systems are rapidly becoming the 
go-to tool for Maritime Commanders to meet their unique needs in the littorals. Some of the 
AUV’s attributes include: 


Human Safety —Since AUVs are unmanned, the risk to human operators is reduced. Addi- 
tionally, they can be used in roles traditionally performed by human divers, thus further 
eliminating the risk to the Navy’s valuable human capital. 

Flexible —Modern AUVs are designed and constructed with modularity in mind, allowing them 
to be outfitted with sensors, actuators, and tools to accomplish a wide variety of missions. 

Mobile —Many AUV systems (vehicle and associated support equipment) are small enough to 
fit into the bed of a pickup truck or onto a helicopter, and are man-portable, allowing them 
to be quickly delivered just about anywhere they are needed. Additionally, their small size 
allows them to maneuver in spaces or areas inaccessible by manned underwater vehicles. 

Autonomous —By definition, AUVs are autonomous, meaning that once set upon it’s mission, 
no human control or intervention is required to accomplish the mission, resulting in lower 
manpower requirements, almost zero risk of danger to human operators, and much higher 
cost savings. 

Low Cost —Cost savings over traditional means for accomplishing the stated objectives are 
reached by use of Commercial Off-The-Shelf (COTS) technologies, reduction of man- 
power requirements, size, and modularity. Further, the minimal amount of support equip- 
ment required for many AUVs is far less than that of a comparably capable ROV, further 


reducing costs. 


1.2 Problem Statement 
The Navy is interested in making AUVs more autonomous. In general, AUV autonomy is 
defined by the ability of the vehicle to interpret it’s environment and plan trajectories that best 


adhere to the mission objectives. It is characterized by four distinct aspects: 


Internal Representation of the Environment - the spatial mapping, or "Environmental Map", 
of objects and obstacles in the environment in which the vehicle is operating 

Path Planning - creating a path to follow; two types of path planning: deliberative, where 
the path is planned based on known information stored in the Environmental Map; and 
reactive, where the vehicle detects objects or obstacles not already existing in the Envi- 
ronmental Map and plans a path to avoid them 

Path Following - Path planning results in a trajectory that the vehicle is trying to follow. This 
provides direction to the controls, which then drives the vehicle along the path. 

Sensory processing -As new sensory information is obtained, such as position and other raw 
sensory data, it is fed back into the loop to localize the vehicle and update the World map. 


Reactive obstacle detection is an example of a behavior exhibiting greater autonomy. The ex- 
ternal representation of the AUV environment could be a map with the water depths and known 
features and obstacles. The path planning module uses this information to plan a path. This 
path must be calculated by considering the limitations in the AUVs maneuverability. Finally, 
sensors such as a forward looking sonar provide imagery regarding the environment forward 
of the AUV. Computer vision techniques are used on the images and pertinent information is 
extracted and entered into the map. This feedback loop continues through the entire deployment 


cycle and must be accomplished in near real-time. [5] 


For this thesis, I am interested persistent AUV autonomy. This is ability of the AUV to remain 
deployed for longer periods of time. The specific concept of operations is to put the vehicle 
into a suspended sleep state. All primary systems are shut down to conserve battery life with 
the exception of a communications or sensing device and the computer to run them. Before the 
AUV goes into this sleep mode, it surveys the area. During the survey it is looking for objects 
readily detected by the forward looking sonar. 


The AUV is eventually awakened via communications or sensory detection. The first objec- 
tive is to localize the position of the AUV. Due to limitations in positive ballast and oceanic 
conditions, the AUV may have drifted along the ocean floor. For tactical and safety reasons, 
it is preferable not to surface the AUV for a GPS fix. The goal is for the AUV to efficiently 
search the area for the navigational fixes that were surveyed prior to the sleep mode. This thesis 
addresses the path planning necessary for such a behavior. The goal is to develop an informa- 
tion theoretic search plan for the AUV to detect the survey sites taking into consideration the 


uncertainty associated with vehicle position due to drift. 


This thesis describes an optimal two-dimensional exploratory path-planning schema for the 
REMUS AUV! that utilizes well known and understood concepts of probabilistic analysis 
(Bayesian inference) and information theoretics (entropic information, divergence measures, 
etc.) for analyzing and quantifying the information gain achieved by an onboard organic sensor. 
It is this latter subject of information analysis that is of key importance to this thesis. Rigorous 
analysis will be given to determining the information gain achieved by the autonomous system 


in the course of following various candidate trajectories. Information gain, in the context of the 





‘Although the methodologies explored by this thesis have been developed for use on the REMUS AUV, they 
are obviously not limited to use on this vehicle, and may certainly be more generally applied, with minimal effort 
or modification, to use in path planning for practically any autonomous agent operating under similar conditions 
and constraints. 


present problem being addressed, represents a quantitative and qualitative measure of the situa- 
tional awareness and utility of the data models being collected and developed within the system. 
This information gain will be utilized as the primary optimized parameter in the exploratory path 
planning and optimization routine. For this, the author will employ a contemporary approach 
to trajectory generation and optimization known as Direct Method of the Inverse Dynamics in 
the Virtual Domain (hereafter referred to as DM-IDVD), that not only ensures complete ex- 
ploratory coverage of the unknown underwater space in which the vehicle operates, but allows 
real-time, near-optimal trajectory generation within the performance constraints of off the shelf 


computing and control systems widely used on autonomous vehicles. 


1.3 Literature Review 

Path planning is one of the most rigorously studied problems in the field of robotics. Numerous 
techniques, methodologies, and algorithms exist to process sensor information and plan a tra- 
jectory to direct an agent (e.g. vehicle, robotic device, etc.) from an initial state to a final state 
subject to dynamic, kinematic, environmental, configurational, or other constraints. The algo- 
rithms and guidance laws produced by such methodologies address such issues as localization, 
track following, cross-track error control, obstacle avoidance, and trajectory optimization. Of 
key importance to the efficacy of almost all of these methodologies, as applied to an autonomous 
vehicle, is the means by which the information gathered by the agent’s onboard sensor(s) is col- 


lected, measured, quantified, analyzed, and applied. 


1.3.1 Trajectory Generation 

In the context of the REMUS AUV, many path planning methods have been explored. Most 
earlier works focused on simplified path planning problems such as contour following and ob- 
stacle avoidance. Van Reet explored contour following techniques utilizing logic-based gra- 
dient methods [6]. Heminger [7] and Healey [8] developed reactive obstacle avoidance tech- 
niques which used Gaussian Potential Functions for obstacle avoidance. Furukawa worked on 
an obstacle avoidance routine that combined a spline addition planner with a look-ahead pitch 
controller [9]. While sufficient for simplified mission tasks, these approaches have intrinsic 
limitations and drawbacks when applied to more complex scenarios, including sub-optimality, 


computational intensity, ambiguity in situational awareness, and sensitivity to error. 


The Direct Methods approach seeks to mitigate these issues, and of late, much attention here at 
NPS has been given to applying this technique to autonomous system navigation. Recent work 


includes that of Yakimenko, Horner, Pratt, and Kragelund. Horner and Yakimenko were the 


first to incorporate Direct Methods into the obstacle avoidance path planning framework. Their 
2007 paper [5] utilized sensor data in a closed feedback loop and a known environmental map 
to provide the situational awareness to the DM-IDVD algorithm. The algorithm then generated 
a near-optimal trajectory for navigation and obstacle avoidance, updating in real time along the 
vehicle’s path. Yakimenko then expanded on this paper in 2008 [10], incorporating the DM- 
IDVD method into a formulation of a more generalized approach to real-time, near-optimal, 
obstacle avoidance, 3D spatial trajectories. Furthering the research into applying the DM-IDVD 
approach to AUV navigation was the paper by Yakimenko, Horner and Pratt which detailed a 
control algorithm for autonomously deploying and recovering AUVs [11]. In this paper, the 
authors apply the DM-IDVD method to calculating trajectory of an AUV out of (deployment) 
or into (recovery) a submerged docking apparatus, reducing the AUVs dependency on the host 
surface vessel or swimmers. What each of these approaches had in common is their use of 
known a priori information of the environment for localization and navigation. The DM-IDVD 


method was applied, more or less, to the task of obstacle avoidance or waypoint navigation. 


1.3.2 Sensor Information Processing 

As alluded to previously, the methods and techniques by which sensor data is analyzed and 
applied is absolutely key to the efficacy of any path planning algorithm. The methodology ex- 
plored by this thesis utilizes existing techniques for handling a sonar image input and converting 
that image into a probabilistic model of the environment. Extensive use is made of McChes- 
ney’s work [12] with manipulating the sonar image collected by the REMUS vehicle’s onboard 
blazed array sonar sensor’. In his thesis, McChesney processes a three-dimensional sonar im- 
age by first developing noise and signal confidence probability models of the sonar sensor itself, 
and then applies a probabilistic update process to develop a three dimensional spatial model of 
the underwater environment. This spatial model is constructed by way of an Occupancy Grid 
(OG). The Bayesian probabilistic methods used to build and update the OG have been rigor- 
ously explored and presented by Elfes [13] and Noykov [14]. The exact method whereby the 


collected sonar image is used to develop the OG will be discussed in detail in Chapter 2. 


In this thesis, information gain (IG) is the key informational parameter under consideration. 
To that end, it must be clarified that the information stored in the OG is merely a means to 


an end. As our primary concern is with determining how much information is gained by the 





*While the McChesney thesis deals with feature reconstruction in three dimensions using both horizontal and 
vertical elements of the BlueView sonar package installed onboard the REMUS vehicle, this thesis will work in 
two dimensions only, dealing with the horizontal plane. 


system for a given candidate trajectory, a means must be utilized with which we can analyze the 
gain, or divergence, in cumulative information between discrete consecutive updates of the OG. 
The tools used in the methodology explored by this thesis come from the field of Information 
Theoretics. Much use is made of the concepts of divergence measures, entropic information, 
and Fisher information, as presented in the context of autonomous agent path planning in the 
Levine thesis [15]. Although Levine applies these concepts to a very different approach to path 
planning (Rapidly-exploring Random Trees), they are equally apropos for the approach used in 
this thesis. 


1.4 Thesis Organization 

Chapter 2 of this thesis will discuss the generalized information theoretics and probabilistic 
methods that will be used to process, update, and quantify the sonar-sensor information. I 
will first describe the conditional probabilistic process of Bayesian inference. This process 
will then be applied to the formulation of the Occupancy Grid (OG) and the update process. 
Next, the critical foundational concepts of information theoretic measures will be discussed. It 
will cover entropic information (conditional entropy), divergence measures (Kullback-Leibler 


divergence), information gain, and Fisher information matrices. 


Chapter 3 provides a generalized discussion of the Direct Methods approach. It is the path 
planning methodology used for the thesis. Chapter 4 will tie the concepts and methods discussed 
in the previous two chapters into a complete path planning routine. First, the methods and 
techniques by which the simulated sonar image is processed into an environmental spatial model 
is discussed. Then the process by which the probabilistic measures of entropy and information 
gain are quantified are elaborated upon. Next, the constraints on the states, controls, and their 
derivatives will be defined. Optimization parameters and the concomitant cost functions which 
incorporate the previously discussed information metrics will then be developed. The candidate 
trajectory reference functions will be presented, followed by a discussion of parameterization 
of the reference functions in the virtual domain. Initial and final conditions on the fixed and 
variable parameters of the reference functions will then be established. The inverse dynamic 
equations are then developed. Finally, with all the pieces in place, the optimization routine 
will be discussed and analyzed. The remaining chapters cover the analysis of the results, the 
findings therein, and finally, the conclusion including a brief discussion of opportunities for 


further research relevant to this thesis topic. 





CHAPTER 2: 


Information Theoretics 





As stated in Chapter 1, the ultimate goal of the approach developed by this thesis is to produce 
an optimal trajectory that maximizes the information gain achieved by that trajectory. Before 
examining the Direct Methods process in detail and describing the approach developed by the 
author, a primer on some of the basic information theoretic measures employed in the author’s 
method. In this chapter, a few of these measures will be reviewed. Bayesian inference is a 
key component of the probabilistic analysis performed during the analysis and quantification of 
the received sonar image and the construction of the corresponding occupancy grid. Entropic 
information is an important metric in determining the value of the information received by the 
system. Divergence measures, such as information gain, enable the quantification of increase in 


knowledge gained during the process, and as stated before, are key parameters for optimization. 


2.1 Probabilistic Measures 


2.1.1 Basic Probability Concepts 

Before discussing Bayesian inference itself, some basic probabilistic concepts must be eluci- 
dated. Let consider a random variable X, and let x denote any specific value that X may be. For 
example, if we consider a simple coin flip, where X is the random variable that represents the 
side of the coin that turns up from the flip, then the possible values of x are heads or tails. The 


probability that X has a value of x is given by 
P(X =x), (2.1) 


and is typically shortened to px(x), or simply p(x). So for the example of the coin flip, the 
probability that heads turns up is indicated by p(X = heads), and the probability of tails turning 
up is p(X = tails). For a “fair" coin, where there is equal probability of achieving either result, 
p(X = heads) = p(X = tails) = 5. Furthermore, discrete probabilities always sum to unity, 


thus )°, p(X =x) = 1 [16]. 


A joint distribution of X and Y is one in which p(x, y) = p(X = x and Y = y), which describes 


the probability of the event where X = x and Y = y simultaneously. If neither random variable 


depends on the other for their respective values, we call the random variables independent, such 


that p(x,y) = p(x) p(y). 


Conditional probabilities are another form of probabilistic measure that give us information 
about the likely value of one random variable given known information about another. In other 
words, a conditional probability is the probability that random variable X takes on a value of x 
given that we know another random variable Y has a value of y. Such probabilities are usually 
denoted 

p(X =x|Y =), (2.2) 


or more commonly 


Pxiy (xly) = ply). @3) 


This is a key fundamental concept, as it will be heavily utilized in the Bayesian inference process 


to be discussed in the next section. Furthermore, if p(y) > 0, then 


v 
Pai 
S 
= 
Wissaste 





p(aly) = a6). (2.4) 


and if X and Y are independent as defined above, we get 


P(x)p(y) 
P(xly) = = = p(x). (2.5) 
Ply) 
This then leads to one last concept relevant to Bayesian inference, the Law of Total Probability: 
If X and Y are independent, and Y is a set of mutually exclusive and exhaustive events (meaning 
only one event can occur at a time, and all possible events are contained in the set), then for any 


other event X, 


p(x) = p(aly1) p01) + P(ly2) pO2) ++ + P(Alyn) POn) = ¥ ply) pO) (2.6) 
y 
in the discrete case. As we will see in the next section, this law forms the denominator of the 


Bayesian Inference equation. 


2.1.2 Bayesian Inference 
Bayesian inference is the core concept of Bayesian decision theory in which information re- 
garding the current state of a random variable is inferred from prior knowledge of states of that 


random variable and present observations. It allows us to iteratively update the specific current, 


or posterior, probability distribution of a random variable based on observational conditional 
probabilities of certain events, and knowledge of the previous, or prior probability distribution. 
Bayesian inference is defined by Bayes rule. Let X be a random variable with possible value, or 
state, x drawn randomly from a set of possible values, 7. Before any observations are made, the 
knowledge of x is given entirely by the prior probability distribution p(x). We model observa- 
tions of the system as random variable Y that take on values of y. Given such observations (e.g. 


sensor data), we may infer the value of x from that of y by way of the Bayes rule equation: 





P(aly) — = y ( (2.7) 
x 


Here, p(x|y) is called the posterior probability distribution and p(y|x) is called the inverse 
conditional probability. This latter component essentially describes the probability of receiving 
value y given a specific value of x, for example, the likelihood that a specific state x value, 


represented here by x’, causes a specific sensor measurement value of y. 


2.2 Entropy and Information Gain 

As previously stated, the objective of this thesis is to plan a trajectory that optimizes the in- 
formation gain achieved by that trajectory. In order to quantify this information gain, we must 
first describe the measure that it is predicated on, the information entropy. In the context of 
information theoretic measures, the entropy of a random variable X, denoted H, is essentially a 
measure of the amount of uncertainty associated with the values of X. It is the expectation of 
the information that each value of the random variable carries, and is defined mathematically 
by equation (2.8) [17]. If X can take on any value x from set of possible values 7, and p(x) is 


the probability of X assuming value x, then the entropy of X is given as 











H(X) = Ex(i(x)] =— Y, p(x) log p(a). (2.8) 


XEX 





where I(x) is the self information of x, given by I(x) = log p(x), which represents the entropy 


contribution carried by each value of x. [17] 


In the special case where random variable X can only take on two possible values, such as the 
case in the coin flip example, the entropy function simplifies to the binary entropy function 
given by (2.9) [17]: 

Hy = —plog p— (1 — p)logo(1 — p) (2.9) 
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Figure 2.1: Binary Entropy 


This function is plotted in Figure 2.1. In this figure, we can see that the maximum entropy 
occurs where the probability that X = 1 (P(X = 1)) is 0.5. (Note that since this is a binary 
function, where X can only equal | or 0, then if P(X = 1) = 0.5, then P(X = 0) = 0.5.) This 
probability value for maximum binary entropy will be used to initialize the occupancy grid 


discussed in Section 2.3. 


Information Gain, also known as Kullback—Leibler divergence, is a measure of the difference 
between two probability distributions: a “‘true" probability distribution p(X), and an “assumed" 
distribution, g(X). If we initially assume that the distribution of values x of random variable 
X is given by g(X), when the actual correct distribution is p(X), then the difference measure 
between the two distributions is given by the Kullback—Leibler divergence, defined as 


Dar(p(2)la(x)) = Y, ~ple)logalx) — (~plwlogpla)) = ¥ plw)log2™ 10) 

xeX xeX q(x) 
If q(x) represents our prior belief of the probability that X = x, and subsequent observations 
(or measurements) reveal a true, or correct, probability distribution p(x), then the information 
gain measures how far from the true distribution our initial belief was. Thus by observing and 
measuring a probabilistic quantity (i.e. random variable) and updating the probability distribu- 
tion, then those observations carry information about that quantity causing us to increase our 


knowledge about that quantity, and this gain is quantified as the information gain. 
2.3, Occupancy Grids 


As robotic sensors are never perfectly accurate, and are thus typically modeled probabilistically, 


a means for generating a high-confidence spatial map of the sensed environment based on this 
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Table 2.1: Occupancy Grid Equation Terms 
Probability that a cell is occupied given 

Foe: FISKE) = Ocenia] the sean all previous eens 
Probability that a cell is occupied given 
all previous measurements 
Probability that a cell is empty given all 
previous measurements 
Probability of receiving the current mea- 
surement given that the cell is occupied 
Probability of receiving the current mea- 
surement given that the cell is empty 








P1 | Pls(C) = Oce|{r};] 





P2 | Pls(C) =Empl|{r},] 





P3 | Plr+i\s(C) = Occ] 





P4 | Plri+1|s(C) = Emp] 

















sensor data must be developed based on probabilistic concepts. Elfes developed an iterative 
method based on Bayesian inference that discretizes the spatial map into a grid of cells. The 
goal is to determine whether each cell is occupied or empty. The occupancy probability of each 


cell given all previous measurements is then given by 


Plr;+1|s(C) = Occ] - P|s(C) = Occ|{r}4] 


Y. Pirils(©] PIO: (2.11) 
Vs(C) 





Pls(C) = Oce|{r}i+1] = 


where 


Tro: The current measurement 
s(C): State of the cell ([Occ]upied or [Emp]ty) 


{r},: All measurements up to time t 


Expanding the summation in the denominator, and subbing in short variable names from column 
1 of Table 2.1, the Bayesian update equation for the occupancy grid cells then becomes 
P3e Pi 


ee 10 
Occ ~ P4. P2+ P3-Pl Ce 





From (2.12), we can see that we initially have 4 unknown variables that we must account for. 
These unknowns are summarized in Table 2.1 [12]. However, recall that the states may only 
have two possible states, Occupied or Empty, so P[s(C) = Occ|{r},]+ P|s(C) = Emp|{r};] = 1. 
So we can substitute 1 — P[s(C) = Occ|{r},| for P[s(C) = Emp|{r};], thereby reducing the 
number of variables required to be calculated for each cell to three. The last two components of 


Table 2.1 are predicated on the probability distributions of the sensor model employed. These 
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Sonar Return Signal Probability Density Functions 





Pls | s(C)}=Emp] 
— — Pfr, | s(C)}-Occ] 





Probability of Receiving Signal Value 








20 500 1000 1500 2000 2500 3000 3500 4000 4500 5000 
Return Signal Value 


Figure 2.2: Sensor Model Probability Distribution Functions 


sensor models probability distributions are typically determined experimentally. An example 
of the probability distributions for the sonar sensor employed on the NPS REMUS vehicle 
can be seen in Figure 2.2. The second and third components represent the prior probability 


distributions of occupancy data already existing in the occupancy grid for each cell. 


Equation (2.12) represents the iterative process that occurs for each cell during each subsequent 
collection of sensor data. Thus an initial value is required for the occupancy probability of each 
cell. Since it is assumed that at time t = 0 we know nothing of the occupancy probability, a value 
that represents this lack of knowledge, or information is needed. Recalling from Section 2.2 that 
this lack of knowledge can be quantified by the informational entropy, we initialize all cells of 
the OG to a value corresponding to maximum entropy. As there are only two possible values that 
the cells can take, both being exclusive and exhaustive, and since the unconditional probabilities 
of the cell taking each of these values are equal, then the corresponding maximum entropy is 
0.5 (see Section 2.2). 


To illustrate the process, consider an example in which a cell begins with an unknown occu- 
pancy state. The prior probability that the cell is occupied, P[s(C) = Occ|{r};], is 0.6, and 
P|s(C) = Emp|{r};] = 1 —P[s(C) = Occ|{r},] = 0.4. Now a sonar sweep across this cell re- 
ceives a return signal, 7+, with a value of 3000. From the sensor probability density functions 
in Figure 4.7, the corresponding values for P[7441|s(C) = Occ] and Plr;+1|s(C) = Emp] are 
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approximately 0.01 and 0.001 respectively. Now, substituting these values into (2.12): 


0.01 -0.6 
Pls(C) = Ocel{r}r+1] = Poce = 9 op7-(1 0.8) 10.0106 70379 





we achieve the posterior occupancy probability of the cell conditioned on the value of the sonar 
signal received, which in this case is equal to 0.9375. Each cell is then updated in the same 
manner using the prior occupancy probabilities and updated with the sensor model probability 


values based on new sensor measurements for each sweep. 
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CHAPTER 3: 
Direct Method of Inverse Dynamics in the Virtual 


Domain 





This chapter describes a method, known as the Direct Method of Inverse Dynamics in the Vir- 
tual Domain (DM-IDVD), whereby a set of candidate spatial trajectories for an autonomous 
agent is generated in real-time for short-term maneuvers of the agent. The trajectories are gen- 
erated by way of chosen reference functions that consist of linear functions of the parameters of 
the system. They are then subjected to an optimization routine that seeks to minimize a given 
cost function by way of built in or predeveloped minimization computational routines or algo- 
rithm, such as fminsearch MATLAB function, or the Hookes-Jeeves minimization algorithm. 
The DM-IDVD process results in the efficient solution of a two-point boundary value problem 
representing the trajectory that is optimal in both the chosen optimization parameter and the set 
of constraints based on vehicle kinematics, obstacle avoidance criteria, etc. The efficiency of 
the routine allows for real-time trajectory optimization within the computational capabilities of 


most modern autonomous systems. [18] 


3.1 Problem Formulation 
The DM-IDVD process begins by assuming that there exists a set of admissible trajectories 
described by the state vector z(t): 


z(t) = [zi (t), za(t),...,z-(t)|? € S 
S={20) eZ CGE hy te |p, tl 
All trajectories in the set must satisfy 
1. the system of ordinary differential equations (typically defined by the vehicle kinematics): 
STR), tS 12 seer 


where w is the vector of controls given by u(t) = {u1(t),u2(t),...,u(t)}?,1<r,weU! Cc 
E', and ¢ is the vector of vehicle technical characteristics given by ¢ = {c1,C9,..., Gyles 
CeCeCc EP; 
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2. initial and final conditions on the states and controls?: 


2(to) =z E So, So{zEZ’ CE} (3.1) 
u(fo) =Up E Ro, Ro = {uo € Cue E'} (3.2) 
z(tr) =z ES, Sp{zpeZ CE} (3.3) 
u(ty) =u eRe, Re={upeU' CE} (3.4) 

3. constraints on: 

(a) state space: 

n(t,z) ={mi(t,z), Na(t,z),---, Nw(t,z)}7 > 0 (3.5) 

(b) controls: 
G(t,z,u) oa (Ei(¢;Z,U),C2(t;2Z,0),.< .,€y(t,z,u)}7 =0 (3.6) 


(c) and their derivatives: 
m(t,z,u,u) = {7 (t,z,u, 8), m(t,z,u,0),...,%o(t,z,u,n)}? >0 = (3.7) 


Given the set of trajectories satisfying these requirements, the objective then is to determine 
the best (near-optimal) trajectory z,p;(t) from within the set and that trajectory’s corresponding 


controls Uy,;(t) that minimize some cost function J. [18] 


3.2 Trajectory Generation 

The next step in the Direct Methods process is to express the candidate trajectories of the states 
as a differentially flat function of some abstract parameter t. (This virtual parameter, typi- 
cally referred to as the virtual arc, will be discussed in more detail in Section 3.3.) Since 
the main idea of direct methods is to consider the solution as a finite set of variables (func- 
tions) [18], we first assume that the admissible functions can be expressed as an infinite power 
series z;(T) = Lipo ajt*, a Fourier series z;(7) = aio /2 + Ve_; (aj coskt + biz sinkt), or more 
generally, by any series function of the form z;(T) = Ly) dix Pix(T) where @j,(T) is any suit- 
able basis function. In fact, the chosen reference functions may be any combination of basis 
functions (e.g. monomials, trigonometric functions, etc.) so long as the set of trajectories gen- 


erated by them meet the requirements in the preceding section. Direct methods then simplify 





3 Although this definition declares the terminal parameters as completely defined, in reality, they may not be. 
In this case, these "free variables" may be added to the set of optimization parameters (OP’s). 
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the problem by considering the solutions to be functions of finite series vice infinite. The solu- 
tion in this case is then merely a function of a set of unknown coefficients. That is to say that 
z(t) = f (aio, Gi1,---;4im, Dio, Di1,---,;Din, T), Where i is the state parameter index and m and n 


are the orders of the basis functions. 


Consider, for instance the general 3D case where the coordinate state vector for an autonomous 
agent is given by x = [x(T),y(7T),z(7)]. The candidate trajectories of the agent can be expressed 


as a polynomial of degree N: 


N 
xi(t) = P(t) = ¥ ait (3.8) 
k=0 


where x1 (T) =x(T),x2(T) =y(T), and x3(T) = z(t). The degree N of the polynomial is depen- 
dent on the number of boundary conditions that must be satisfied so that all coefficients aj are 
determined algebraically instead of being varied. The variable T is left as a varied parameter. 
In general, the minimum order n of polynomial required is determined by the orders of the time 


derivatives of the initial and final coordinates (dp and dy respectively): 
n=dj+dr+1 (3.9) 


Boundary conditions at the initial and final (terminal) points of the desired trajectory that must 
be satisfied typically include constraints on initial and final position, velocity, and acceleration, 
LC. Xio,XigsXig Xif Xip.X;p [19]. In the case that each of these represents a given boundary 
condition to be satisfied, then the minimum order N of polynomial P in (3.8) is 5, since dp = 
dy = 2. Thus all of the coefficients of P will be uniquely defined by these boundary conditions, 
leaving T as the only varied parameter. Of course, if needed, higher order derivatives may be 
added to meet desired states at initial and/or final points. This simply requires increasing the 
polynomial degree by the number of additional constraint derivative orders. For example, fixing 
UL 


the final jerk x; at zero (as is the case in many terminal trajectory problems) increases dy by 1, 


requiring a polynomial of order 6. 


Thus far, we have considered the virtual arc T as the only varied parameter. This, however, 
limits the flexibility of our reference functions, as it provides only one optimization variable for 
varying our reference trajectories within the set of admissible trajectories. Greater flexibility 
may be achieved in our reference trajectory by increasing the number of varied parameters to 
be considered in the optimization routine. This can be accomplished by "fictitious" boundary 
conditions at the end points. For example, to increase the flexibility in the reference trajectories 
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of the 3D terminal trajectory case discussed above, we may add a 3rd order derivative XOp> or 


jerk constraint, to the initial point. Thus do becomes 3, and N = dy +d +1 =7. This additional 
"fictitious" derivative can now be used as a varied parameter, giving us greater control over the 


shape of the candidate trajectories. 


3.3. The Virtual Domain 


We now turn our attention to the reason behind parameterizing the reference functions with re- 
spect to an arbitrary, or virtual parameter Tt. Up to this point, we have only concerned ourselves 
with the flexibility of the trajectory reference function itself. We must now consider why pa- 
rameterizing the reference functions with respect to time ¢ actually limits our flexibility. Let us 
consider what would happen if this were indeed the case by analyzing the speed profile of the 
resulting time-parameterized trajectory. In this case, the speed at any time ¢ along the trajectory 


would be given by 


Vie [ult)?-+ v(t)? + w(t)? HVP4+P42=4/P2+P? +P? (3.10) 


Thus for any candidate spatial trajectory given by P, we have a single unique and unalterable 





speed profile along that trajectory. This is undesirable since we might want to be able to vary 
the speed profile independently of the spatial trajectory. By parameterizing the trajectory with 
respect to an abstract argument, in this case the virtual arc parameter T(t At), we introduce 
a speed factor that allows for just such independence between speed and the spatial trajectory. 


This speed factor A is given by 





dt 
A(t) =—, 3.11 
(= 5 G.I) 
which is essentially the virtual speed. Now since 
dx; dx;(T) d 
eps AEE. (3.12) 


dt ie ae 


we have 





V(t) =A(t)y/x(0)2 +y!(0)? +2(0)? = A(t) /P2 + P+ PP (3.13) 


Thus by varying the speed factor 2, we’re able to vary the speed profile independently of the 


spatial trajectory. 


The reason that the capability to vary the speed profile is desirable warrants further discussion. 


In most cases, the minimum and maximum speeds of a vehicle are constrained by Ving, and 
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Vinin- In the case of the time-dependent speed given by (3.10), the direct correlation between 
the spatial trajectory and speed along the trajectory means that the speed constraints will also 
constrain the spatial trajectory, severely limiting our set of candidate trajectories. Computing 
the trajectories in the tT domain, however, allows us to satisfy these constraints by adjusting 
A(t) without directly affecting the spatial trajectory, such that 


Vimin SA(t)4) BPE RPP < Vinax- 


Likewise, consider possible constraints on centrifugal acceleration a°’ = kV, where k is the 
curvature of the trajectory. Here again, solving in the time domain ties the spatial trajectory 
directly to speed V, and thus a°’. Moving to the tT domain, however, allows us to again satisfy 


this constraint independently of the spatial trajectory by adjusting A(T), since 
a‘! (t) = k(t)V7(t) = R(T)A*(t)(PE +P + PP) < aia: 


Yakimenko [20] makes note of some important points concerning this virtual parameterization. 
First, if the virtual arc length Ty is on the order of physical path length s, then the virtual speed 
A(t) will be correspondingly congruent with the physical speed along the trajectory. Thus if 
we set Ap equal to Vo, we can expect Tr ~ sf. This conclusion enables us to make a sound 
initial guess on T, during the optimization portion of the routine. Second, we can see from the 


relationship between 7 and time given by equation (3.11) that time can be expressed as 


= [ dt 
0 A(t) 
Thus varying the speed factor A (7) not only changes the speed profile, but the time scale as well. 
Finally, we can use the relationship between the virtual domain and time domain in equation 


(3.11) to determine the derivatives in the time domain of any time-variant parameter [21]. Given 


parameter C, the first and second time-derivatives of this parameter are 


aba _ 


= AS (At) and Ot) =A(A'C' +46") (3.14) 


C(t) 


We may then invert equation (3.14) to transfer the boundary conditions from the time domain 


into the T domain: 


ta na & NE 
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3.4 Inverse Dynamics 

Once the set of candidate trajectories has been computed, the next step in the DM-IDVD routine 
is to compute the components of the state vector and the controls vector at discrete points along 
each candidate trajectory. In so doing, not only do we ensure that the constraints of (3.5)—(3.7) 
are not violated, but we also explicitly return the controls required to follow each candidate 
trajectory. What we are essentially doing is computing the time-histories of the states and 
controls along the trajectory. This is accomplished by way of inverting the dynamic equations 
of motion for the vehicle. Let us again consider the case of a simple 6 Degree of Freedom 
(6DOF) vehicle operating in 3D cartesian space. Suppose the kinematics of the vehicle are 


given by 


y(t) | =pR | vit) (3.16) 
a(t) w(t) 
where }R is the rotation matrix from the body frame {b} to the NED local tangent plane frame 
{u}, and is defined by two Euler angles, pitch (@(t)) and yaw (w(t)) (for simplicity, we will 
neglect roll angle (@(t)): 


cos w(t)cosO@(t) —siny(t) cos w(t) sin @(t) 
pR= | sinw(t)cos@(t) cosw(t)  siny(t)sin@(t) (3.17) 
—sin 0(t) 0 cos O(f) 


Before applying inverse dynamics, we must transfer (3.16) to the t domain (assuming a constant 


surge velocity Up): 


x(t Uo 
A(t) | y(t) | =pR] v(t) (3.18) 
Ze w(T) 


If we also assume that pitch angle is small enough such that sin @(t) + 0 and cos 0(t) + 0, (3.17) 


becomes 
cosy(T) —siny(t) 0 
pR(T) = | sinw(t) cosw(t) 0 (3.19) 
0 0 1 


Note that as all kinematic parameters and equations have now been converted to the T domain, 


for simplicity the notation for functional t-dependency will be hereafter omitted. Inverting the 
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kinematic equations (3.16) produces the following inverse dynamics equation: 


Up cosy siny 0 x 
v | =A] —-sinw cosy 0 y’ (3.20) 
w 0 0 1 Z 


The remaining inverse dynamics equations are arrived at by solving (3.20) for the three un- 


known parameters, v, w, and y. By inspection, we can readily ascertain that 
w=Az, (3.21) 


whereas the solutions for the other two unknown parameters are not as easily achieved. By way 
of a geometric analysis of the scalar product of the vectors on the right hand side of (3.20), we 
arrive at the following solutions for v and y: 








v= [2 (x? + y?) —U? (3.22) 

and ia 

Vv Vv 
—W—tan! = W— tan! — 3.23 
yw an Uo/h an Us ( ) 
where 
y’ 

Y =tan! =. (3.24) 


With the kinematic equations thusly inverted, we are now able to verify that the constraints on 


these states are not violated by the candidate trajectories. 


3.5 Reference Function Coefficients 

States at the boundary conditions are now evaluated so that we may determine the coefficients 
of the reference functions (3.8). Initial and final conditions on the coordinates (xj9 and x;f) are 
given by equations (3.1) and (3.3). Known or given boundary conditions on velocity compo- 
nents of surge, sway, and heave define the first-order t-derivatives of the coordinate states (xg, 
x;) by way of (3.18): 


Of Uo 

ee pee (3.25) 
Bi Ao: f Of : 

LOsf Wo; f 
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where the initial and final values of yaw required to determine ;Ro,¢ above are given by (3.23): 


Wo.p = Yo — tan! OL (3.26) 
Uo 
and those for pitch are found by: 
Oo.¢ = +tan-1—___ OF (3.27) 


7 a 
,/U6 +VG.5 


The second order derivatives at the initial point (x/4) are typically provided by vehicle motion 
sensors (accelerometers), although they may be explicitly stated as a given initial condition, 
while the final second order derivatives (xi) are usually given as defined desired final con- 
ditions on acceleration. Likewise, the initial and final higher order derivatives required by the 
chosen reference functions are are either assigned "guessed" values if they represent the variable 
parameters (OP’s), or are explicitly given as dictated by the constraints on trajectory behavior 
or constraints on the controls at the initial and terminal points. All of the derivatives must be 
converted into the tT domain as required by the method describe above. With all necessary 
states and their tT derivatives have been defined for the initial and terminal points, along with 
guesses for the variable parameters (including Ty), the coefficients are then found by solving 


the appropriate system of equations defined by (3.8) for these coefficients. 


3.6 Discretization 
In order to numerically calculate the remaining states over the length of the virtual arc from 
T=0 to T= Ty, it is necessary to discretize the trajectory into N evenly spaced points (in the T 


domain) with intervals of 





Apt SE (3.28) 


such that 
a= TAtAt, JH2,...,N, (n=). (3.29) 


The Ar for each interval may now be determined: 


(xj —xj-1)? + (9; —yj-1)? + (zj —Zj-1)? 
ee) 5) 
Up oe A a 





Atj—1| = sqrt eo eee (3.30) 


ee 


The speed factor A for each interval by way of the discrete version of (3.11): 


AT 


i= 
Z Atj—1 





(3.31) 


The value of A in (3.25) may be assumed to be any reasonable value, such as Ao,¢ = 1, since 
this value merely scales the virtual domain. In other words, the higher the value assigned to A, 
the larger the resulting value of ty. This result is due to the time-virtual domain relationships 
given by (3.11) and (3.13). Thus we can see that as = where s+ is the total physical path 
length along the trajectory. [10] Once the trajectory has been discretized and A; for each point 
computed by (3.31), we may compute the states and controls at all intermediate point at each 
time stamp along the trajectory by way of the inverse dynamic equations determined previously 
((3.21) through (3.23) in the 6DOF 3D vehicle case above). In some cases, numeric analysis 
may be required to calculate the time derivatives of certain parameters in order to perform 
constraint checks on these parameters. Such is the case with ¥ in the 3D example case above, 
where we must numerically differentiate (3.24) in order to check a given constraint on ‘¥ such 
as | (t)| <Pmax. The net result is a time-history of the states and controls at uniform discrete 
points along the trajectory. 


3.7 Optimization 

Obviously, the whole of the entire above process generates the necessary set of trajectories sub- 
ject to (3.1) to (3.4), we must now turn our attention to satisfying the constraints given by (3.5) 
through (3.7), along with any optimization parameters required by the situation. As the goal of 
Direct Methods is to determine a single near-optimal trajectory subject to these constraints and 
OPs, we must invoke some sort of optimization routine that will optimize a defined performance 
index and penalty (cost) function. This routine will accept the set of trajectories (more precisely, 
the state and control time histories), constraints, and OPs as input, and output the trajectory that 
most closely (near-optimally) satisfies the given requirements. 


Let us first consider the cost function. We desire that our solution trajectory meet certain re- 
quirements or not violate certain constraints. The cost function is how we quantify such con- 
straint violations or satisfaction of requirements. The cost function determines how much a 
given constraint or requirement is violated, and calculates an associated penalty value. Let’s 
consider, for example, an example of an Unmanned Underwater Vehicle (UUV), defined by the 
equations discussed previously in the 6DOF 3D case. Suppose we place the obvious constraints 
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on vehicle depth z (measured in the NED coordinate frame) 


zmin <zj <0, (3.32) 
pitch 0 

}9(1)| < Onax, (3.33) 
and yaw rate yw 

W(O)| < Winax- (3.34) 


Penalties for each may be expressed as 


my (0; z; —Zmin)” 





min (0; —z;)* 

: (3.35) 
max (0;|0;| — 6; Hee and 

j 
sr (0; | Wil — YW; eye 

The resulting cost function multiplies these by a related scaling (weighting) parameter k: 
: : 2 
Nae (032; =. Zinit) 
min (0;—z;)” 
AS le, Kk, KY | j ; (3.36) 
ese (0; y;| = Wins) 





Other parameters that may be penalized subject to minimums and/or maximums might be speed 
components (Ummin << Uj <Umaxs Vmin < Vj < Vmax, etc.), bank angle (19; < Qmax), or the controls 


(|5;| < Smax), as well as their derivatives (actuator dynamics) (d;| Gay: 


The performance index factors in those parameters that are to be minimized or maximized based 
on the specific requirements of the problem, such as final time t, or some other parameter spe- 
cific to the application, such as in the case with this thesis, Information Gain. In this case, since 
we will attempt to maximize the IG, the performance index for IG will simply be a weighting 
factor w’S times the actual calculated value for IG. 
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Once the overall cost function and performance index is formulated, optimization proceeds by 
utilizing some optimization algorithm, such as those mentioned at the beginning of this chapter. 
An example of this, and the method chosen for use in this thesis, is the fminsearch algorithm in 
MATLAB. Optimization is achieved by inputing initial guesses for the variable (optimization) 
parameters, choosing the required number of iterations, and running the fminsearch algorithm 
on the function that contains the trajectory generation code, the inverse dynamics calculations, 
and the combined cost function/performance index (hereafter referred to simply as PI). The rou- 
tine then seeks to minimize the PI by varying values of the "guessed" optimization parameters. 
The end result is a solution containing the final guesses for the OP’s and the time-histories of 


the states/controls corresponding to the near-optimal (minimized) PI. 


i) 


THIS PAGE INTENTIONALLY LEFT BLANK 
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CHAPTER 4: 
Algorithm Implementation and Testing 





This chapter describes the path planning algorithm for the REMUS AUV developed for this the- 
sis. The algorithm implements the previously described concepts of Direct Methods, Bayesian 
inference, probabilistic analysis, occupancy grids, and information gain. The algorithm takes 
a known "world map" of the environment as input, as well as user specified constraints and 
boundary conditions on the vehicle’s kinematic and spatial parameters. The net result is a tra- 
jectory that satisfies the specified constraints on the vehicle and optimizes the information gain 
achieved by the vehicle’s sensors. Figure 4.1 shows a simplified block diagram of the routine. 
The orange box is the DM-IDVD path planning and optimization routine. The white box within 
this box constitutes the optimization loop that is run within the optimization function. The gray 
bubble corresponding to the gray box in the optimization loop displays the probabilistic analysis 
subroutine, which performs the sonar imaging, occupancy grid updating, information gain cal- 
culation, heuristics violation analysis, and obstacle avoidance subroutine. Each component will 
be further described in this chapter. The entire routine is run using a set of MATLAB scripts and 
functions developed by the author specifically for this thesis, as well as a couple of off-the-shelf 
scripts sourced from the Mathworks website which offer simple functionality for specific tasks 
(such as generating a Bresenham line in the sonar sweep and obstacle avoidance subroutines). 
All of the pertinent MATLAB code used in this thesis is presented in Appendix A. 


4.1 Problem Setup 

The simulation environment chosen for this thesis was the testing tank located in the basement 
of Halligan Hall on the campus of the Naval Postgraduate School. The tank measures approx- 
imately 10 meters by 20 meters by 2 meters deep. The tank is modeled in MATLAB in matrix 
form for the purposes of generating simulated sonar images and the occupancy grid. All posi- 
tional coordinates of the vehicle are analyzed with respect to the center of gravity of the vehicle, 


which, for the sake of simplicity, corresponds to the vertical (z-axis) center of rotation. 


As will be discussed in Section 4.2.1, the simulation was run with the vehicle starting with a 
known and fixed set of initial conditions on position and heading. While the choice for the 
initial state is completely arbitrary, that for the final condition is not. In theory, the number of 


candidate endpoints is quasi-infinite, bounded only by the constraints of the test space which, 


27 








Compute Reference 
Function 


Aetna Coefficients beam aia 





Compute States & Controls Via 
Inverse Dynamics 











Perfarm Sim Sonar 
Sweep and 


Change values Check 
of variable Index Constraints 
parameters Vv 


ct — ~~ Met 
Tol Tolerance 


olerance \, 
eal Not Met Not Met —_ 
4 









































Figure 4.1: Overall Trajectory Generation and Optimization Routine 


in the case of this thesis, is the test tank. Although the simulation constrains the trajectories of 
the vehicle to the confines of the test tank, a means must be addressed by which an endpoint of 
the set of candidate trajectories evaluated by the direct methods routine may be chosen from the 
quasi-infinite set of endpoints. Thus another constraint, a time horizon (TH), is incorporated. 
The TH is a notional period of time over which we will limit the vehicles motion and perform 
the required analysis. All trajectories of the candidate trajectory sets will be confined to the set 
of endpoints that the vehicle can possibly reach given constraints on it’s surge (u) and sway (v) 
velocities. Since Uma, is greater than Ving, for the REMUS vehicle, the set of possible endpoints 
is confined to lie within an arc represented by the TH multiplied by the una. A graphical 
representation of the random endpoints can be seen in Figure 4.2. 


Now, while it is possible to implement the DM-IDVD trajectory optimization routine with free 
endpoint coordinates given as variable parameters, the computational requirements for such a 
routine would be exponentially increased, as this also leaves the first order time derivatives of 
the coordinates at the endpoints free to vary as well. Thus this approach is not feasible for this 
application, especially given the computational burden already placed on the resources by the 
subordinate components of the routine added to the standard DM-IDVD algorithm by this thesis 
(i.e. the sonar imaging, probabilistic analysis, and information theoretics). Thus, when setting 
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Figure 4.2: Randomly Generated Endpoints (red circles are the endpoints, black star is the start point; 
pink circle is the time horizon arc, blue rectangle is the test tank) 


up the problem, it became necessary to further constrain the endpoint coordinates to a smaller 
region. In order to accomplish this, an initial analysis of the information space was performed 
to determine where the endpoints are that achieve the greatest information gain for simple tra- 
jectories. This was accomplished by generating a set of random endpoints that lie within the 
confines defined by the tank walls and the TH arc. A simplified iterative routine was run that 
traverses the vehicle from the fixed starting point to each of the random end points. Each end- 
point trajectory was also iterated over a fixed number for randomly generated final yaw angles 
(wy), in order to maximize our knowledge of the information space over all variable parameters. 
A map of the information space for each value of Wy was generated by evaluating the informa- 
tion gain achieved along that specific trajectory. A simplified algorithmic representation of this 
method is shown in Algorithm 1. The resulting map is a 3D representation of IG as a function 
of the endpoint coordinates for each value of wr. Two dimensionally, an example of this can 


be seen in Figure 4.3. This figure is a "heat map" where each cell of the figure represents 
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Algorithm 1 Information Space Initial Analysis Algorithm 





1. For i = 1 to number for random W;’s 
2. Generate empty IG Array 
3. For k = 1 to number of random endpoints 
4. Generate straight-line trajectory between start and endpoint(k) 
5. At each discrete point along trajectory: 
6. Perform sonar sweep subroutine to generate sonar image 
7. Run OG Update subroutine on sonar image 
— Update OG 
—> Calculate IG at this point and add it to previous IG value 
8. Store cumulative IG(k) value in cell corresponding to endpoint(k) 
9. end loop 
10. end loop 





a cartesian coordinate and the value information gain of each coordinate is represented by a 
color, with cooler colors representing lower values and hotter colors representing higher values. 
A 3D version of this information gain mapping can be seen in Figure 4.4. By inspection of this 
IG mapping for each of the values of wy, it became clear that maximal information gain was 
achieved with trajectories terminating in the Southwest corner of the TH-constrained operating 
space. Thus the values for the final coordinates are placed in this general location throughout the 
rest of the algorithm testing. In a real-world application where this a priori data is not achiev- 
able, other methods, such as heuristic evaluations, known-space-constrained explorations, etc., 


may be used to determine the desired end point. 


4.2 Direct Methods Problem Formulation 


4.2.1 Definition of States, Constraints, and Boundary Conditions 
As discussed in Section 3.1, the first step was to define the vehicle state vector, controls, and 
equations of motion in state space form. As this thesis analyzes only two-dimensional trajecto- 


ries in the horizontal plane, the state vector x was defined as 


x(t) = [x(t), y(t), wt), w(t), oO)’, (4.1) 





u(t) 
4.2 
| (4.2) 
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Figure 4.3: Information Gain Heat Map for wr = —45° (NED) Red colors indicate higher IG values, 
blue colors indicate lower IG values 


where }R is given by 
cos y(t) —sin w(t) (43) 


BRI) = siny(t) cos y(t) 


The controls vector is given by 


u= [A prop: N fit Nsit] (4.4) 


where Nprop, Nfir, ANd Nj; are the controls (thrusts) on the main propeller, forward lateral cross- 
body thruster, and stern lateral cross-body thruster, respectively. Next, the boundary conditions 
at the initial and final points were defined. The initial pose of the vehicle was arbitrarily defined 
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Figure 4.4: Three Dimensional Map of Information Gain for wr = —45° (NED) Red colors indicate 
higher IG values, blue colors indicate lower IG values 


with a fixed starting point and heading: 


t=0 


a 
II 


YO = Yinit 
W(t = 0) = Wo = PSiinit 
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where Xinit, Yinit, ANd PSijnj are the values for these parameters specified in the main MATLAB 
script (see Appendix A). Likewise, the final pose is defined by: 


x(t = tf) =Xf = Xfinal 

y(t = t 0) = Vf = Yfinal 

y(t = tf) =Wr= PSi final 
While the value used for psifingi was initially chosen somewhat arbitrarily in the interest of 
simplifying the problem (and later implemented as a variable parameter), as discussed in Sec- 


tion 4.1, the choice of values for x fing; and yfingi 1S not arbitrary. These values were chosen 


based on the initial information space analysis described in that section. 
Kinematic constraints of the vehicle were sourced from Doherty [22]: 


Table 4.1: Kinematic Constraints 





Constraint Value 
Max Surge Velocity (Vinax) 2.88 m/s 
Max Sway Velocity (Umax) 0.5 m/s 
Max Yaw Rate (Winax) 14.5 deg/sec 























4.2.2 Reference Functions 

The first step in the DM-IDVD process, represented by the first block in Figure 4.1, is to choose 
the reference functions for the key spatial parameters of the trajectory. The key parameters 
of concern in this case were Euclidean spatial coordinates x and y, and yaw angle y. Thus 
reference functions for these parameters were developed. Additionally, a reference function 
for the speed factor A was also chosen to allow for maximum flexibility in the speed profile 
along the trajectory. As discussed in Section 3.3, the reference functions are parameterized 
in the virtual domain as a function of the virtual parameter t. The reference function for 
the coordinates x; (i = {1,2}, sox = x; and y = x2) are shown in equation (4.5) below. The 
chosen reference function for the spatial coordinates is a third order polynomial combined with 
two trigonometric sine functions. The addition of the sine functions increases the flexibility in 
varying the curvature of the resulting trajectory. This reference function is then differentiated 


twice to allow the final acceleration in each coordinate to be varied. (The resulting cosine 
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terms in the second equation of (4.5) arise from the first derivative of the sine terms in the first 


equation.) 
x(t): P(t) =a,+a)t+a)t? +4)? +b) sin(xt) +b) sin(227) 
x(t): te! (4) =a), +257 + 3a4t? + 2bj cos(xT) + 2b} cos(227) (4.5) 
x"i(t) 2 T2P)"(4) = 2a + 6a3t — 2°b' sin(xt) + 4275 sin(277) 
where 
a8 
t= — 
Uf 


The parameter T is used as opposed to T in order to simplify the process of solving for the 
coefficients at T) and Ty in the sine and cosine components of the reference function. The 
same parameter was used for all of the reference functions for consistency, despite the lack of 
trigonometric terms in the wy and A reference functions. The next steps given by equations (4.6) 
and (4.7) substitute T) and Ty for T into (4.5) to develop the matrix equation (4.8). (Note that 
although equations (4.6) through (4.8) are those for the x coordinate, those for y are of the same 


form.) 


x 


)|z=0 > X09 = do 
Jiro: Tx = aj + 1b} + 20b4 (4.6) 


al 


x( 
x! ( 


x (ea62 T FXO = 2a; 


al 


al 


x( 
x 


MD) ane TX} = 2a} + 6a 


late XP = AQ +a, +a, +a; 


al 


Yieaep  Tpx'p = a] + 2a} + 303 — 1b} + 20d} (4.7) 
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The above equations are then put into matrix form as seen in (4.8): 


130: ODE Qe a x0 
0100 na 2 ai Xoty 
x M 2 

ae OO 20 OF % _ | rot (4.8) 
11141 40 0 a Xf 
0123 -a 2a bi xipTf 
0026 0 0 bs XUTE 


The following boundary conditions, as well as the initial guesses for the varied parameters 
xf and Ye are then substituted into (4.8) and the reference function coefficients are solved for 
(second step of DM-IDVD illustrated by the second block of Figure 4.1). 


X0 = Xinit Af * final 

xp =0 xp =0 (4.9) 
x, =0 xp = var 

YO = Jinit Yf =Yfinal 

yo =0 ve =0 (4.10) 
yo =0 ye = var 


where Xinit, X final» Yinit, ANd Yfingi are the chosen trajectory start point and end point values 
discussed in Section 4.2.1. The reference function for y was chosen to be a simple fifth order 
polynomial. Since it was desired to vary the initial and final second derivatives of y, a fifth order 
polynomial was necessary due to the requirement given by (3.9). The development process of 
the reference function including solving for the coefficients is identical to that described above. 


Reference Function for y: 


W(t) =Py(t) =a) taftt+aPt+afP+alt+al? 
w(t) = tpPy!(t) =a¥ + 2a¥ t+ 3a¥ 7? + dal + 5a¥e4 (4.11) 
wi" (t) = t7Py" (t) = 2ay +. 6a} 7+ 12a} t? + 20a¥t° 
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As before, T and T, are substituted into (4.11) to get the system of equations corresponding to 
the boundaries: 


W(T)|-0: Youd 
W'(T)|e0: TW=al (4.12) 
W"(T)|r-0: TFY = 2a¥ 


a 


al 


W(T)|r=t, : Wr =ay tay tay +a} tay +al 
W (Flat: Tey = af +20} + 3a¥ + 4af +5a¥ (4.13) 
W'(BDleace: Tew! = 2a¥ + 6a + 12a¥ + 20a¥ 

f fs 2 3 4 5 


al 


1000 0 0 ay Yo 
0100 0 0 av W' otf 
0:02 10-0 30 . Note 
=> “2 | = W 0% (4.14) 
TD hide Mis Se id a; Wr 
Os eee ae ae ay Wi! Te 
y 
00 2 6 12 20] | a yw" TF 
The boundary conditions on y and the first and second derivatives are: 
Wo = PSiinit Wer = PSI final 
yw =0 yw, =0 (4.15) 


Yo = var Wy = var 


where, as before, the values for psijnj, and PSi fing) Were developed in Section 4.2.1, and the 
initial and final second derivatives are left as varied parameters. As with x and y, these bound- 
ary conditions along with the initial guesses on wi and Wi are substituted into (4.14) and the 
coefficients solved for. 


The reference function for the speed factor A is of the exact same form as that for y, and is 
identically developed, and thus will not be described in detail here. The boundary conditions 
on A are also of the same form, with Ag = lamin and Az = lam fing being set equal to one. 
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This allows for a direct correspondence between the speed factor and initial and final speeds, as 
discussed in Section 3.3. The initial and final first derivatives of A are set to zero, and the initial 


and final second derivatives are variable parameters. 


4.3 Inverse Dynamics 

Once the reference functions are fully developed as functions of Tt as discussed in the preceding 
section, they are evaluated over tT = (0; ty]. The result is a fully defined and discretized tra- 
jectory in x, y, and y, with the necessary t-domain derivatives being evaluated concomitantly. 
The next step, illustrated by the third block of Figure 4.1, is to compute the remaining states 
and controls via inverse dynamics. Values for heading (‘V), path angle (8), surge velocity (uw), 
sway velocity (v), total speed (V), and time (t), as well as the time derivatives of the angles are 


calculated at each discrete point k along the trajectory by the following set of inverse dynamic 











equations: 
a 
AT | 
t(1) =0 
t(k > 1) =t(kK-1)+At 
is 2AT 
AES EAB 
t(1) =0 
t(k > 1) =t(k-—1)+<dt (4.16) 
= x'(k) 
W(k) =tan Ga) 
B(k) =¥(k) — w(k) 
V(k) =A(k)y/ (&/(k))? + "(KP 
u(k) =V (k) sin(B(k)) 
v(k) =V (k) cos(B(k)) 
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Figure 4.5: Trajectory Angles in the Horizontal Plane 


and the time derivatives of the angles are calculated by 





; x! (k)y"(k) —y'(k)x"(k 
YO CREEP 
W(k) =A (k)'P"(k) (4.17) 
W(k) =A (k) w'(k) 
p-¥—w 


The angular relationships are arrived at by the motion dynamics of the vehicle in the horizontal 
plane. Consider Figure 4.5. In this figure, V is the velocity vector of the vehicle in the NED 
local tangent plane (LTP), and u and v are the surge and sway velocities respectively. The yaw 
angle y is defined as the direction that the positive x-axis of the vehicle in the body centered 
frame {b} makes with the Northerly axis of the LTP. The heading angle V is the angle between 
the North axis of the LTP and the velocity vector V. B is defined as the angle between the the 
velocity vector and the positive x-axis of the vehicle in {b}. Thus B = ¥ — y. 


4.4 Path Probabilistic Analysis 


The probabilistic analysis portion of the routine occurs immediately after the inverse dynamics 


calculations. It consists of several subroutines that are executed at each discrete point along each 
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trajectory. This part takes as inputs the vector of coordinate values (x, y) of the geometric center 
of the vehicle along the entire trajectory, and the corresponding vector of yaw angles y for each 
point. It executes subroutine functions at each point for performing a sonar sweep, updating the 
occupancy grid, and calculating information gain, as well as performing the necessary analyses 


for heuristics violations and obstacle avoidance. 


4.4.1 Generating the Sonar Image 

In order to generate a simulated sonar image of the environment, a "World Model" was first 
constructed to represent the spatial environment that the vehicle was to be operated in. For this 
thesis, that environment was to be a model of the Center for Autonomous Vehicles Research 
(CAVR) water test tank located in the basement of Halligan Hall on the NPS campus. This open 
top tank measures approximately 10 meters by 20 meters. The tank is modeled in MATLAB 
with a simple matrix. The matrix cell values at and outside the tank walls have a value of 1, 
while those inside the walls (representing water space) have a value of 0. The sonar image 
is generated by a MATLAB function that performs a sonar sweep against the World Model 
(sweep.m). The sonar sweep function casts a single sonar ray along a Bresenham line that has 
a length equal to the effective range of the sonar. If all World Model cells along this ray hold 
a value of zero, then the function chooses a random value from an equally distributed range of 
possible signal strength values for a “no-object-in-beam" condition. Conversely, if a cell with 
a value of 1 is found along the ray, then the function chooses a random value from an equally 
distributed range of possible signal strength values for a “no-object-in-beam" condition. The 
ranges for “object-in-beam" and “‘no-object-in-beam" signal strength values are bounded by the 
minimum and maximum likely signal strengths for each condition. In the “object-in-beam" 
case, the function then determines the World Model matrix coordinates for the first cell along 
the ray with a value of 1, and stores the returned signal value in the Sonar Image matrix cell 
corresponding to the same coordinates of the World Model. This ray-tracing routine is repeated 
for a discrete number of rays about the entire swath of the sonar sensor, which, for the FLS 
on the REMUS, is +45° of the bow. An example of the generated cumulative sonar image, 
with one object in the tank, is shown in Figure 4.6. This image is a compendium of all of the 


individual sonar images developed for each sonar sweep for the entire trajectory. 


4.4.2 Updating the Occupancy Grid 
The occupancy grid is initialized as a matrix with the same dimensions as the given World 


Model, with all cells assigned the value of 0.5 (see Section 2.3). It is then updated at each point 
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Sonar Image 





Figure 4.6: Sample Cumulative Sonar Image 


along the trajectory by an OG update subroutine (OGupdate.m). This function takes the sonar 
image generated in the previous subroutine and performs the Bayesian inference described in 
Section 2.3 and defined by equation (2.12) to update the occupancy probability of each cell. In 
order to obtain P[r;41|s(C) = Occ] and P[r;+1|s(C) = Emp], a probabilistic model of the sensor 
must first be built. This is performed by yet another subroutine function (SensorModelGenera- 
tion.m). In this function, the probabilities that a given sensor value is obtained given either an 
“object-in-beam" or “no-object-in-beam" state are modeled as continuous probability density 
functions (pdf). Each of these models was generated based on data from McChesney*. The 
pdf’s for the sonar sensor model are plotted in figure Figure 4.7. The occupancy grid updater 


processes each individual cell of the sonar image and determines the P[7;41|s(C) = Occ] and 





4McChesney utilized histogram models for sensor modeling. Continuous pdf’s were used in this thesis due to 
the lack of availability of his histogram data and insufficient time for actual in-tank sensor data collection. For the 
objectives of this thesis, the models used are deemed sufficient for proof-of-concept testing. 
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Sonar Return Signal Probability Density Functions 
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Figure 4.7: Sensor Model Probability Distribution Functions 


P\r;41|s(C) = Emp] based on the signal value stored in the cell and the probability value of re- 
ceiving this signal value based on the generated sensor model. The remaining unknown variable 
in equation (2.12), P|s(C) = Occ|{r}+], is the previous occupancy probability value of that cell. 
The newly calculated occupancy probability value for this cell will then be the prior value for 
the next iteration of the Bayesian inference. Thus as more sonar sweeps across each cell happen, 
the occupancy probability for each cell is constantly being updated, eventually converging on 
the actual occupancy state of the cells. In this way, the occupancy grid is constantly increasing 
in confidence. A sample occupancy grid, based on the data used to produce the sonar image in 


Figure 4.6 is shown in Figure 4.8. 


4.4.3 Calculating Information Gain 

The same function that updates the occupancy grid also calculates the incremental information 
gain at each step. As the occupancy probability of each cell is continually resolved by the 
Bayesian inference, our knowledge of the actual state of each cell is constantly increasing, while 
the information entropy is correspondingly decreasing. By applying the concepts discussed in 
sections 2.1.2 and 2.2 the amount of change of probability values of each cell at each iteration 
directly corresponds to the information gain achieved during that iteration. The information 


gain is thus calculated during each update as the difference between the prior and posterior 
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Figure 4.8: Sample Occupancy Grid 


occupancy probabilities of each cell. The net information gain over the entire trajectory is then 


the cumulative sum of the information gain achieved over every iteration for all cells. 


4.4.4 Heuristic Boundaries 

At the start of the routine, the vehicle starts with no information about its environment. Even 
after the first sonar sweep, the only knowledge it has is a low-confidence probabilistic estimate 
of what lies in the sonar cone of that initial sweep. In order to allow for freedom of motion at this 
beginning phase of the path, certain assumptions must be made about the region immediately 


surrounding the vehicle. 


In this vein, an initial “box" of free space is established around the vehicle so that it may 
maneuver through unknown space until sufficient data has been collected to permit normal 
exploration-based maneuvering based on collected data. The heuristic bounding box assumes 
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that the walls detected during the initial sonar sweep extend beyond the edges of the sonar cone 
by a fixed amount sufficient to allow for an assumption of no objects or obstacles in a path 
traveling to the immediate left or right of the vehicle. The forward looking sonar dictates a bias 
towards forward exploratory motion, thus heuristic boundaries astern of the vehicle originate at 
the sternmost point of the vehicle. With these considerations in mind, one side of the heuristic 
bounding box at the vehicle’s stern is defined as a North-South line in the local tangent plane 
(LTP) frame originating at the sternmost point of the vehicle and extending in the N-axis direc- 
tion corresponding to that of the N-axis component of the vehicle centerline vector. The other 
stern-side is similarly defined in the E-W direction in the LTP. The remaining two sides are sub- 
ject to the shape of whatever is seen during the initial sonar sweep, and thus are more difficult 
to explicitly define. It should be noted that if a secondary sensory system were utilized, such as 
a side-scan sonar, the augmented sensory coverage to the port and starboard side of the AUV 
would likely provide sufficient information about the adjacent environment at the start of the 
routine such that these heuristic boundaries would be unnecessary for an AUV with cross-body 


thrusters. 


In the case of this thesis, however, the vehicle starts in a corner of the tank, so the first assump- 
tion above is applied to the walls of the tank seen during the initial scan. The remaining two 
(forward) heuristic boundaries are then defined by extending the lines of the walls seen by the 
sonar beyond the sonar cone. These lines originate at the corner where the right and upper tank 
walls meet, extending beyond the initial sonar spread and terminating where the left and lower 
boundaries intersect with the walls. The resulting boundaries form a box that can be seen as 
the red dotted lines in Figure 4.9. These boundaries are used in two ways. First, the occu- 
pancy grid cells along the heuristic boundaries at the walls are initialized to a value of 0.75, 
vice 0.5. Increasing this initial value reduces the possible information gain achieved by sensing 
the walls within the heuristic bounding box, thus weighting subsequent explorations away from 
these wall sections. Basically since we are already assuming the existence of the wall along 
the boundaries, we want the information gain accrued when sensing these walls to be smaller 
than that achieved by exploring unknown areas. Second, the boundaries at the stern of the ve- 
hicle are used to penalize the vehicle for backing down into unknown (unexplored) territory. 
At the outset, the area astern of the vehicle that lies beyond the stern heuristic boundaries is 
completely unknown, and so there obviously exists the possibility for obstacles or obstructions 


in those regions. Any generated trajectory that causes the vehicle to back down into this space 
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Figure 4.9: Heuristic Boundaries, represented by the red-dashed lines. The blue rectangle represents 
the test tank walls; the pink square is an obstacle in the tank; The red box represents the sonar cone 
of the FLS. 


before it has been explored is penalized proportionally to the extent by which the stern violates 
this boundary. The penalty function for this will be discussed further in Section 4.5. 


4.4.5 Obstacle Avoidance 

An obstacle avoidance subroutine is necessary to prevent the vehicle from coming into contact 
with any obstacles or objects in the water. Since a vehicle with cross-body thrusters, such as 
the NPS REMUS, is capable of significant lateral translation, it is not sufficient to base the 
avoidance criteria simply on obstacles ahead of the vehicle. Thus a method was developed that 
circumscribes a circular “avoidance perimeter" about the centroid of the vehicle with a radius 
equal to the length of the vehicle. Each trajectory is then analyzed at each point along the 
path for any intrusion of known object boundaries within this intrusion perimeter. The penalty 
assigned to this intrusion is proportional to the penetration depth into the perimeter. The data 
used for “known" objects is sourced from the OG, meaning that only objects or obstacles that 
have been “seen" by the sonar are used in this analysis. This fact, combined with the sideslip 
capability of the vehicle, led to the requirement for two other subroutines that bias the trajectory 
into explored space, and limit the amount of sideslip into unexplored space. These subroutines 


will be discussed further in Section 5.2. 
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4.5 Optimization Scheme (f/minsearch) 

The routine now proceeds with the optimization portion of the DM-IDVD process. A cost 
function (CF) that calculates penalties for violating given constraints is calculated, as well as a 
performance index (PI) of the optimized parameter, information gain. As stated previously, the 
entire calculative routine is evaluated within MATLAB’s fminsearch() algorithm. This function 
is a gradient-free minimization routine that seeks to find a local minimum of the input function. 
In the case of the DM-IDVD routine, the input function f(x) (trajectory.m), consists of all 
calculations examined thus far, and terminates with the calculation of the proceeding CF and PI. 
The value to be minimized is thus the arithmetic sum of these two trajectory function outputs. 
The fminsearch function also requires an initial guess on the neighborhood of values for the 
variable parameters in the input function. It then iterates by varying the values of these variable 
parameters until either the local minimum of the function value f(x) maximum (user-specified) 
number of iterations is reached. Values are also specified for the convergence tolerance for both 
f(x) and x. 


4.5.1 Cost Functions and Performance Index 
Penalties in the cost function were initially specified for the final time, yaw rate, surge velocity, 


sway velocity, heuristics violations, and allision avoidance: 


Time: Penalty for exceeding desired trajectory time. This desired time is spec- 
ified as the time horizon TH. Since this isn’t a critical penalty, a lower 
relative weighting factor is assigned to it. 


W: Penalty for exceeding max yaw rate. 
U: Penalty for exceeding max surge velocity. 
V: Penalty for exceeding max sway velocity. 


Heuristics Violation: Penalty for violating heuristics boundaries. This prevents backing into 


the unknown region astern of the vehicle at the start of the trajectory. 


Allision Avoidance: Object/obstacle proximity penalty. The closer the path comes to an ob- 
ject or obstacle, the higher this penalty. This is based on a specified 


avoidance radius centered on the vehicle centroid. 
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Table 4.2: Cost Function Components 


























Penalty Formula 
Time Costr = (time(end) — TH)? 
y Finey = max(|0, (abs(y) — Winas) |) 
u Fine, = max((0, (abs(u) — umax)|)” 
v Fine, = max([0, (abs(v) — Vmax)])? 
Heuristics Violation | Costy = ViolArea* 
Allision Avoidance | Cost,p = AP** 














Notes: *—ViolArea is the calculated are between the lower heuristic boundary and the arc swept by the vehicle’s 
stern in violation of this boundary. **—AP is the calculated by a subroutine that assigns a cumulative penalty of 
all of the individual proximity violations at each point along the trajectory, which are directly proportional to the 
magnitude of the incursion of the object into a circle described about the vehicle centroid with a given avoidance 


radius. 
Table 4.2 show the formulas used in calculating the penalties in the cost function. 


The performance index is simply the inverse of the cumulative information gain for the gener- 
ated trajectory, and is calculated as Costjg = iG The inverse is used to maintain consistency 
with the other cost functions in that it is desired to minimize each within the fminsearch func- 


tion. Thus minimizing the inverse of IG means that IG will, in fact, be maximized. 


Each of these CF/PI factors is multiplied by a weighting factor and summed. This sum of prod- 
ucts is the value that is actually minimized by the fminsearch function. Once the minimal value 
is found, the "optimal" trajectory that produced it is returned, including all state parameters (x, 
y, and psi), as well as the states and controls calculated by the inverse dynamics. These outputs 
are the final product of the routine—an optimal trajectory that satisfies all constraints imposed 


on the problem and maximizes the optimization parameter, Information Gain. 
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CHAPTERS: 
Data Analysis and Findings 





The routine described in Chapter 4 was built and run entirely in the MATLAB computing envi- 
ronment. All pertinent MATLAB functions and scripts are contained in Appendix A on page 73. 
The following sections discuss the numerical results of the simulation, as well as the resulting 
trajectories for three simulation scenarios of the REMUS vehicle in the test tank with zero, one, 
and two obstacles in the path of the vehicle. Constraints on surge (Umax), SWAY (Vmax), and yaw 
rate (Wimax) were sourced from Doherty [22]. Initial guesses on the varied parameters and their 
corresponding final optimal values for the trajectory generation runs are shown in the corre- 
sponding tables for each set of runs. Prior to running the full simulation, the initial guesses 
were made empirically by running several simulation runs with a reduced set of cost functions 
(IG, EV excluded) to validate the trajectory generation routine. The final values for the varied 
parameters were used as a baseline for subsequent complete optimization runs. These values 
were adjusted as required until consistent results at the end points were observed. The simu- 
lation was then run repeatedly, varying the initial guesses of the variable parameters as well as 
the values for the weighting factors for the cost functions and performance index. The resulting 
state and kinematic parameters were then observed, and any adjustments required to amelio- 
rate or minimize constraint violations were made to the weighting factors. This process was 


repeated until a satisfactory trajectory was obtained. 


The following sections discuss the three scenarios analyzed: no object in the tank, one object 
in the tank, and two objects in the tank. Three representative runs from each scenario are 
discussed, including the values for the variable parameters and weighting coefficients, as well 


as the results for each run. 


5.1 Scenario 1: No obstacles 
The set of runs for the first scenario were made with no obstacles in the tank. Values for the 
pertinent variable parameters for each run are shown in Table 5.1. The values for the weighting 


factors for each run are shown in Table 5.2. 
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Table 5.1: Variable Parameter Initial Guess Values for Scenario 1 





2 10 
Run 1 | 10 | 1 | 0.7854 | 0.5 | 2.3562 | 0.126 | -0.054 | 0.0017 | 0.0009 | -1.5708 rad 

1 

1 








Run 2 | 10 0.7854 | 0.5 | -0.7854 | 0.2 | -0.05 | 0.017 | 0.0009 | = -1.5708 
Run 3 | 10 0.7854 | 0.5 | -0.7854 | 0.126 | -0.054 | 0.017 | 0.0009 | -1.5708 












































Variable Parameter Key: 


. Virtual Arc Length (T,) 
. Magnitude of final acceleration (m/s7) 
. Direction of final acceleration (radians) 


. Magnitude of initial acceleration (m/s) 


. Initial yaw acceleration (y’) (rad/s) 
. Final yaw acceleration (We ) (rad /s*) 
. Initial A” 
. Final A” 

10. Final yaw angle (radians) 


1 
2 
3 
4 
5. Direction of initial acceleration (radians) 
6 
a 
8 
9 


Table 5.2: Cost Function Weighting Coefficients for Scenario 1 
Lede e Sei 4 5 6|7)8 
Runl | 1/10; 1) 10 1 ;10/;1)1 
Run2}1/;10;/1)/;100; 10 ;}10; 1/1 
Run3 | 1/10/11); 10 | 100; 1 | 1/1 















































Weighting Factor Key: 


Time 

. Yaw Rate (W) 

. Surge Velocity (uv) 

. Sway Velocity (v) 
B 

. Heuristics Violation 


. Allision Avoidance 


CANADA KWH 


. Explored Space 


The set of runs of the full routine for scenario | were done with the vehicle initially positioned 


in the upper right section of the tank. The center of the vehicle was positioned one and a half ve- 
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Figure 5.1: Scenario 1, All Runs, Initial Pose 


hicle lengths from both the top and right (East) walls of the tank, and the yaw angle was chosen 
to be +45° from due North, thus pointing the vehicle at the corner of the tank, as illustrated in 
Figure 5.1. This pose was chosen to simplify the heuristics assumptions and to put the vehicle 
into the maximally constrained initial state. The end point was then chosen to conform to the 
initial information space analysis discussed in Section 4.1. The probabilistic analysis portion of 
the routine, which included the sonar imaging subroutines, Bayesian inference, occupancy grid 
updating, heuristics analysis, and obstacle avoidance subroutines, was found to be quite compu- 
tationally intensive, causing very long run times for a sufficient number of iterations. Because 
of this, the runs were initially performed without the probabilistic analysis of information gain 
to allow for quick validation of the basic setup of the algorithm. With no object in the tank, 
and no probabilistic analysis, the routine initially generated unsatisfactory trajectories. Without 
the sonar imaging and resulting occupancy grid, there was no capability for allision avoidance. 
Thus several of the initially generated trajectories actually crossed the boundaries of the tank 
walls before reaching the final point. Run | of this set is an example of this result, as seen in Fig- 
ure 5.2. The values for the varied parameters and CF weighting coefficients were then adjusted 
and another set of runs were made. The trajectory for Run 2, illustrated in Figure 5.3, resulted 
in trajectories that remained within the confines of the tank, but violated constraints. With the 


trajectories at least now remaining within the tank, the probabilistic analysis subroutines were 
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Figure 5.2: Scenario 1, Run 1 Trajectory 


reimplemented back into the overall routine, and another set of runs was performed. Again, 
the CF weighting factors were repeatedly adjusted in an attempt to eliminate any constraint vi- 
olations, however this was not achieved in this scenario. Figure 5.4 shows the best trajectory 
achieved for the initial conditions in this scenario, and the corresponding occupancy grid can 
be seen in Figure 5.5. Figure 5.6 shows the violation of the max sway velocity (v), while Fig- 
ure 5.7 shows the max yaw rate violation for this run. Subsequent attempts to ameliorate this 


problem failed to generate a fully satisfactory trajectory that met all constraints. 


5.2 Scenario 2: One Obstacle 


The set of runs for the second scenario were made with a single obstacle in the tank placed in 
the nominal path of the vehicle. Values for the pertinent variable parameters for each run are 
shown in Table 5.3. The values for the weighting factors for each run are shown in Table 5.4. 


Table 5.3: Variable Parameter Initial Guess Values for Scenario 2 


1 |2 A 4 BS) 6 7 8 9 10 
Run 1 | 10 | 1 | 0.7854 | 0.5 | 2.3562 | 0.126 | -0.054 | 0.0017 | 0.0009 | -1.5708 
Run 2 | 10 | 1 | 0.7854 | 0.5 | -0.7854 | 0.126 | -0.054 | 0.0017 | 0.0009 | -1.5708 
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Figure 5.3: Scenario 1, Run 2 Trajectory 


Table 5.4: Cost Function Weighting Coefficients for Scenario 2 
1;2|3)] 4 | 6 |7)| 8 


Runl;1/10}1/;10) 1 | 10; 1) 1 
Run2)/;1/10}1{10) 100; 10] 1) 1 












































Although no satisfactory runs were made in the initial scenario, an attempt was made to continue 
testing with a single obstacle in the tank, in the hopes that further adjustments to the pertinent 
parameters combined with an obstacle in the tank would drive down the constraint violations. 
Initial conditions on the vehicle position and pose for the first two runs of this scenario were 
the same as those for the first scenario, illustrated in Figure 5.1. Figure 5.8 shows an example 
of the resulting trajectory with the parameters set as indicated in Tables 5.3 and 5.4. As seen in 
this figure, the generated trajectory starts in the Easterly direction. Numerous attempts made to 
modify the initial guesses for the initial acceleration angle to eliminate this behavior were fruit- 
less. Trajectories that were forced to start in the Northwesterly direction continued to violate 


kinematic constraints. 


When the initial guesses on the variable parameters were adjusted to force the start of the tra- 
jectory away from the corner, the algorithm repeatedly generated trajectories that caused some 


degree of allision with the obstacle during its transit to the final point. Most of these trajectories 
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Figure 5.4: Scenario 1, Run 3 Trajectory 


were predominantly side-slip trajectories, with little surge velocity. Inspection of the resulting 
penalties revealed an unusual result: the value of the allision avoidance penalty was zero. Upon 
viewing the generated sonar images and trajectory animation, the reason became clear—in order 
to satisfy constraints on time and side slip, the generated trajectories were fairly close to linear 
trajectories between the initial and final point, with a small amount of curvature. However, due 
to constraints on maximum yaw rate, the vehicle was not able to slew around to the point where 
the object was in the sonar field of view. Thus the vehicle was not “‘seeing" the obstacle at all. 
Figure 5.9 illustrates one such trajectory in which the stern of the vehicle actually allides with 
the object in the tank. Looking at the occupancy grid for this run shown in Figure 5.10, it is 
clear that the vehicle never even saw the obstacle. Two new penalties were then added to the 
cost function to account for this. First, a penalty on B, the angle between the yaw angle and 
heading, was specified. This fine penalizes the trajectory any time the magnitude of B (|B]) is 
greater than 45°. This tries to keep the heading of the vehicle within +45° of the positive x-axis 
of the vehicle, thus favoring trajectories that “drive into" the sonar cone. Upon implementation 
of this, however, it was noted that, due to the flexibility of the trajectory in side-slip allowed by 
the cross body thrusters, the algorithm may still generate trajectories that travel in a direction 
parallel to one side of the sonar cone (i.e. B + 45°) at some point along the trajectory. Thus 
an obstacle might still lie in the path of the vehicle, and the sonar would never see it. So a 
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Figure 5.5: Scenario 1, Run 3 Occupancy Grid 


second additional cost function penalty was added to favor trajectories that caused the centroid 
of the vehicle> to only traverse through "explored space" that has been swept by the sonar. The 
formulas used for these penalties are given in Table 5.5. 


Table 5.5: Formulas for Added B and Explored Space Cost Function Penalties 























Penalty Formula 
B Fineg = max((0,(abs(B) — Bmnax)])” 
Explored Space | Costgy = 1/(1+EV*) 








>The centroid was chosen for simplicity of calculation within the routine. In theory, any point aft of the centroid 
along the vehicle centerline would be sufficient to impart a “moment” on the vehicle that would help "steer" it into 
explored territory. 
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Figure 5.7: Scenario 1, Run 3 Winax Violation 


Notes:*—-EV is calculated by a separate subroutine that determines if, and by how far, the vehicle centroid lies 


outside of areas on the OG that have values not equal to 0.5, and thus have been swept by the sonar. 


As before, some amount of adjusting of the weighting coefficient for these cost function terms 
was performed. However, satisfactory results were never obtained. The addition of the two ad- 
ditional penalties caused even more “competition” between the components of the cost function, 
and finding the right balance of weighting coefficient values proved to be incredibly difficult. 
Continued adjustments of the initial guesses on the variable parameters, especially those on yj// 
and the direction of initial acceleration, failed to sufficiently ameliorate this behavior. It seemed 
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Figure 5.8: Scenario 2, Run 1 Trajectory 


that it may be physically impossible to start with the given initial pose of the vehicle and steer 
around an obstacle to a final point that lie to the SouthWest of the vehicle, while minimizing 
violations of the set of physical constraints on u, v, and W, as well as the heuristics. Further ad- 
justments to the initial guesses on the variable parameters did improve the situation somewhat, 


but the generated trajectories still ended up being unsatisfactory. 


Careful consideration was given to the reason for the failure of the algorithm to generate satis- 
factory trajectories. Clues were found when adjusting the weighting coefficient on each physical 
constraint during attempts to reduce violations of these constraints. For example, increasing the 
coefficient for the sway velocity to bring v within the v,,g, constraints consistently caused a 
larger violation of the constraint on y. This lent credence to the hypothesis that the initial con- 
ditions were far too geometrically restrictive. Essentially, this meant that, with the initial pose 
of the vehicle facing the corner, it was practically physically impossible for the vehicle to tra- 
verse from the starting pose and position to the arbitrary final point. Thus the initial conditions 
were modified such that the vehicle was initially placed in a pose at the same starting coordi- 
nates, but facing due North. Promising results were obtained when starting from this pose, as 
the constraint violations immediately became much smaller, and so experimentation was then 


continued with this new starting pose. 
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Figure 5.9: Scenario 2, Run 2: Vehicle hits object in tank 


5.3. Scenario 3: Two Obstacles, North-facing Starting Pose 

With the initial conditions now given by the North-facing starting pose, as shown in Figure 5.11, 
satisfactory results were eventually obtained for the single-obstacle case. The next step was to 
place two obstacles in the tank to determine if the algorithm would generate a satisfactory trajec- 
tory between both obstacles while optimizing the information gain. The same values from the 
immediately preceding runs were used for the initial values for the weighting coefficients and 
variable parameters. Additionally, the resulting animated plot of all of the initial two-obstacle 
runs revealed that the vehicle favored an initial rotation to starboard (clockwise), leading to 
many of these large violations. No amount of adjustment of either the weighting coefficients or 
variable parameter initial guesses eliminated this behavior. Again, the vehicle seemed to be too 
tightly physically constrained. The relatively short distance of just a few meters between the 
start point and end point, combined with the extra obstacle to contend with, proved too restric- 
tive for the algorithm to handle given the physical constraints imposed on the vehicle. Again, 
another change was made to the boundary conditions, this time at the end point. The time hori- 
zon was increased to allow for a larger final range of motion, and the trajectory end point was 
adjusted accordingly, towards the West end of the tank. The two obstacles were moved such 
that they allowed for a sufficient impediment to travel between the start and end point. With this 


revised setup, the simulation was run, and the results were immediately clear—greatly reduced 
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Figure 5.10: Scenario 2, Run 2 Occupancy Grid 


physical constraint violations, and no allision with any object or wall. An example of an early 
run from this scenario can be seen in Figure 5.12. As before, initial testing resulted in some 
non-zero violation penalties. Most significant in the early runs were the fact that the vehicle 
still alluded with the upper obstacle. 


Subsequent runs with adjusted values for the variable parameter initial guesses and weighting 
factors continued to improve the resulting trajectory and CF penalty values. An example of 
a better run can be seen in Figure 5.13. The vehicle successfully avoids both obstacles, but 


constraints in both v and y were still significant, as seen in Figure 5.14 (v), and Figure 5.15 


(y). 
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Figure 5.11: Scenario 3, All Runs, Initial Pose 


Although the initial results of runs using the revised initial setup, with the Northward vehicle 
pose and increased trajectory length, were promising, they still did not meet the requirements 
for success. Physical constraints were still violated, and the performance index penalty was not 
low enough (approx. 4 x 10~*). As before, adjustments were made to the weighting coefficients 
until satisfactory results were finally obtained. Values for the variable parameter initial guesses 


and weighting coefficients can be seen in Table 5.6 and Table 5.7. 


Table 5.6: Variable Parameter Initial Guess Values for Scenario 3 





10 

Run! | 10) 1 | 0.7854 | 0.05 | -2.3562 | 0.126 | -0.054 | 0.0017 | 0.0009 | -1.5708 
Run 2 | 15 | 0.5 | -0.7854 | 0.5 | 2.3562 | -0.5 | -0.054 | 0.0017 | 0.0009 | -1.5708 
Run 3 | 15 | 0.5 0 0.25 | 2.3562 | -0.1 | -0.054 | 0.0017 | 0.0009 | -1.5708 


















































Table 5.7: Cost Function Weighting Coefficients for Scenario 3 
1; 2 |3 4 5 6 d 8 
Runl}1/ 10 / 1 10 1000 | 10 1 1 
Run2/1/; 1 | 1 10 1 1 1 1 
Run 3 | 1 1 | 10000 1 1 | 0.01 | 10 















































Figure 5.16 displays the final near-optimal trajectory of the vehicle. The initial point is in- 


dicated by the red star to the upper right. As can be seen, the vehicle successfully navigates 
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Figure 5.12: Scenario 3, Run 1 Trajectory 


towards the defined end point without alliding with the fixed obstacles or the walls of the tank. 
Figure 5.17 shows the corresponding occupancy grid for the final successful run. Although this 
trajectory successfully navigated the trajectory with no constraint violations and an optimized 
PI, examining Figure 5.17 reveals that the sonar didn’t actually “see" the lower obstacle from a 
planning perspective. From Figure 5.18, we can see that the vehicle, constrained in yaw rate, 
was not able to slew around sufficiently to sweep the sonar across the lower obstacle. A more 
complex trajectory with different values on the constraints would be required to see this obsta- 
cle. However, given that the algorithm successfully generated a trajectory that satisfied all of 
the requirements, notably the constraints and performance index optimization, this trajectory is 


deemed to be a valid near-optimal trajectory for the given conditions. 


5.4 Computational Performance 

One major issue that was readily apparent during the entire process was the computational in- 
tensity of the routine. Specifically, the subroutines associated with the sonar sweep, occupancy 
grid updater, and obstacle avoidance were especially intensive in both cpu cycles and memory 
requirements. Figure 5.19 shows output of the built in MATLAB profiler for the top 20 func- 
tions used in executing the entire algorithm routine. The profiler profiles every line of code as 


it runs and documents number of function calls, self time, and overhead time. By inspection, it 
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Figure 5.13: Scenario 3, Run 2 Trajectory 


is readily apparent that just a few of the subroutine functions utilize a lions share of the com- 
putational time, and the impact is significant. In the final run that generated the satisfactory 
trajectory detailed in this section, the total computation time was 50 seconds®. As seen in the 
profiler output, 40% was used for the occupancy grid calculations (OGupdate), 37% for the 


sonar imaging (sweep), and 21% was used in the obstacle avoidance subroutine (Avoidance). 





©All runs were performed in MATLAB 7.12.0.635 (R201 1a) 32bit, on a PC running Microsoft Windows XP 
Professional with a 2.66GHz Intel Core2 Duo CPU with 3.25GB of RAM. Attempts were made to run the algorithm 
on an NPS distributed computing cluster, however, the MATLAB license on the cluster prevents actual distributed 
computing, and so minimal performance gains were actually realized. 
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Figure 5.14: Scenario 3, Run 2 vq, Violation 

















Figure 5.15: Scenario 3, Run 2 Wingy Violation 
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Figure 5.16: Final Near-Optimal Vehicle Trajectory 
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Figure 5.17: Occupancy Grid for Near-Optimal Trajectory 
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Figure 5.18: Vehicle Orientation at Several Points Along Final Near-Optimal Trajectory 
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Function Name a Calls Total Time Self Time* | Total Time Plot 
(dark band = self time) 

fminsearch 1 3051.526s 0.2515 a 

trajectory 235 3051.213s 1.7515 ee) 

OGupdate: 235 1242796s 1155515s 

swee| 23500 1133.336s 1021.028s il 

Avoidance 23500 647.725s |311.098s Mm 

bresenham 10622000 448.935s 448.935s Ml 

polyval 4486742 87405s 87.390s 1 

sym.subs 940 22.236 s 0.080 s | 

sym.subs>mupadsubs 940 21.130 s 0.046 s | 

sym.subs>tryFunctionHandle 940 21.084 s 0.345 s | 

mupadmex (MEX-file) 3978 19.255 s 19.255 s | 

sym.char 940 16.293 s 0.125 s | 

InitMatrices 235 2.745 s 2.745 s 

sym.subs>getUnknowns 940 1.872 s 0.078s 

sym.subs>makeFhandle 940 1.157 s 0.750 s 

sym.subs>getVars 940 1.026 s 0.110s 

RefFuncs 1 0.969 s 0.078s 

sym-findsym 940 0916s 0.094 s 

sym.subs>evalableY 940 0.872 s 0.201s 

cell.setdiff 1880 0.735 s 0.296 s 











Figure 5.19: MATLAB Profiler Output for Final Optimal Trajectory Run 
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CHAPTER 6: 


Conclusions and Recommendations 





6.1 Conclusions 

As seen in Chapter 5, valid results were obtained for the problem of generating an optimal 
trajectory that not only optimized the key parameter of interest, Information Gain, but also 
satisfied all constraints imposed on the trajectory, such as kinematic and obstacle avoidance 
constraints. However, the stated goal of generating these optimal trajectories in real time may be 
jeopardized by the computational intensity of the algorithm. As discussed in Section 5.4, just a 
few MATLAB routines occupied a significant share of the computation time. These subroutines 
are key to the sonar imaging, probabilistic analysis of information gain, and obstacle avoidance. 
As each of these routines performs numerous calculations sequentially on individual cells of 
large matrices, some computational rigor is expected. An important aspect to consider, however, 
is that much of the intensive computation being performed by MATLAB in the simulation 
environment will actually be done by dedicated hardware on the REMUS vehicle. For example, 
the sonar imaging that consumed so much computation time in MATLAB will be performed by 
the Blueview FLS and it’s dedicated image processing hardware. This alone would drastically 
reduce the load on the algorithm itself. Similar gains would likely be achieved by other aspects 
of the hardware on the vehicle, especially given that compiled code runs orders of magnitude 
faster than MATLAB interpreted code. 


A key observation made during the algorithm test and evaluation phase of this thesis was the 
sensitivity of the trajectory to the adjustments made to the weighting coefficients and variable 
parameter initial guesses. Many adjustments had to be made to several of these parameters 
in order to generate physically valid trajectories for each scenario. In some cases, adjusting a 
weighting coefficient to minimize one penalty resulted in an increase in another penalty. An 
example of this is when adjustments made to reduce the B violation penalty resulted in higher 
yaw rates, increasing the Wax violation. Further thesis research is required to investigate the 


interplay between, and sensitivity of, each of the variable parameters and weighting coefficients. 


Another interesting observation is that the generated trajectories were often not intuitive in that 
they were frequently non-linear and deviated significantly from the relatively simple trajectories 


expected for reactive obstacle avoidance. As discussed in the thesis, the DM-IDVD routine 
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generates full sets of candidate trajectories by varying kinematic parameters at the boundaries 
of the trajectory, such as speed, acceleration, etc. This feature, combined with the fact that 
the coordinates, yaw angle, and speed factor of the trajectories are represented by reference 
functions combining high order polynomials and trigonometric functions, produces these that 


are far more flexible and complex, both spatially and kinematically. 


In conclusion, this thesis successfully proved the concept of utilizing the Direct Method of 
Inverse Dynamics in the Virtual Domain to calculate information-optimal trajectories based on 
a probabilistic analysis of information gain. However, the viability of utilizing this method for 
real-time trajectory generation was not established. Time constraints precluded further code 
optimization and compilation into executable code to be run on the vehicle, and so in-tank 


testing on the REMUS vehicle itself was never achieved. 


6.2 Recommendations for Future Work 

As discussed in the conclusions, there may well be significant opportunity for performance 
improvements by optimizing the MATLAB code used to program the routine. Careful scrutiny 
might be given to specific functions and subroutines within the algorithm to determine where 
and how code optimizations may be made to improve the efficiency of the routine. Hardware- 
in-the-loop simulation may also significantly speed up the computations in the algorithm as 
well. Specifically, the author recommends examining alternative methods for the following 
subroutines/functions: ray tracing (performed in both the sonar imaging and allision avoidance 
subroutines); occupancy grid cell probabilistic calculations (specifically where the sensor signal 
probabilities are evaluated from the sensor model probability density functions); and symbolic 
math manipulations, specifically wherever the MATLAB polyval() functions is utilized. 


Following any required code optimizations, the obvious next step should be to compile the 
algorithm into computer code compatible with the computer resources on the REMUS vehicle 
and perform actual in-tank testing in the CAVR AUV test tank. Following the completion of 
successful in-tank testing, open water testing would then be needed to ascertain the viability of 


the routine in the presence of disturbances. 


Concomitant with on-vehicle testing of the algorithm would be implementation of methods 
required to reduce real-world positional uncertainty caused by sleep-state drift and inherent 


sensor error and noise. As methods for performing this task, such as Kalman filters, are already 
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well known and developed, augmenting the algorithm developed by this thesis with a positional 
uncertainty reduction scheme should be fairly trivial. 
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APPENDIX A: 
MATLAB Code 





A.1 Main Function: Main_opt.m 


Bk KKK KKK KK KKK KKK KK KKK KKK KK KK RK KK KK KK KR KKK KE KK KKK KR KKK KKK KKK RK KKK KKK KKK 


% 
% 


° 
oO 


JAP AP PoP oP Ps OW 


ae 


MAIN_OPT.M 
THIS IS THE MAIN SCRIPT. RUN THIS TO PERFORM THE FULL SIMULATION. 














Written by: LT Adam Wiseman 

All code was written by the author with the exception of bresenham.m and 
the code for generating the animated plot. Credits for bresenham.m are 
inside that script, and credit for the animated plot code and the 
associated scripts/functions code goes to Dr. Les Carr of the Math Dept. 
at NPS. 


Last Edited: 23 March 2012 





eee ee ee ee ee ee ee ee ee ee ee ee ee ee ees 


6% Prep—clear workspace, declare global variables 


ear all 











ose all 








cl 
cl 
cl 


LC 


profile on —history 








lobal L DWH R psi_dot_max u_max v_max dt TH MN 





lobal xinit yinit psii beta_max Spsif 





lobal OG Snrimg 








QQ Q a 


lobal AnimFlag ObsFlag ProbFlag x y VARS 





%% Switches—turn certain features on/off 


AnimFlag=1; 


oe 





Set to 0 to disable display of the REMUS animation 





ProbFlag=1; & Set to 0 to run routine without probabilistic analysis 


& (speeds up trajectory validation process) 


ObsFlag=1; %& Set to 0 to disable obstacles in tank 


%% Vehicle and Tank Dimensions 


L=2.75; % vehicle length in meters 


2 


D=0.19; % vehicle diameter in meters 





W=20; & tank length in meters 
H=10; & tank width in meters 
R=2x*H; & This is used to control size of tank matrix oversize and 


%& sonar beam range. It was originally set to the 





& range of the sonar, but this resulted in overly large data 
% matrices, and was thus reduced in length, with no detriment 


S$ to the overall routine within the constraints of the test 





% tank dimensions. Future testing in larger environments will 


ol? 


require this to be set back to the sonar range. 


M=10* (2*R+H); % Row space for model grids/matrices 


N=10* (2*R+W); % Column space for model grids/matrices 





ol? 


Note: The multiplication factor of 10 allows for finer grids in the 


ol? 


ol? 








ol? 





addressing. This multiplication factor is also found in some of the 


ol? 


subroutines that require coordinate—to-—matrix cell index translation. 


6% Initialization of parameters 
Sxxxxx*x Time Horizon (S) *x*xx*x** 
TH=15; %& Nominal time of trajectory from start point to end point. 


BR KKK KEK KEK KK KKK KKK KKK KK KKK KK 





dt=.1; % Time step (s) 

rfi=5; % Refinement factor for refining data mesh. This simply 
% decreases the step size of each maneuver. Raise this 
% value to reduce the step siz 

IG=[]; 6 Initialize Information Gain (for computational speed) 





fe) 


% Constraints 
dp=14.5; 
pSiTH=TH«dp; 


ol? 


Max yaw rate (deg/sec) 


ol? 


Max yaw over time horizon (deg) 


ol? 


psi_dot_max = dpx*pi/180; maximum yaw rate, rad/s 


ol? 


v_max = .5; maximum sway velocity, m/s 


ol? 


u_max = 2.88; 


beta_max=45*pi/180; 


surge velocity in m/s 





ol? 


absolute value of max desired beta 


ol? 


(to keep trajectory within sonar cone) 


S$x*xx*xINITIAL PARAMETER VALUES! ! !x«x«x«x 
xinit=W—3«L/2; 


ol? 
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sonar image and occupancy grid, and minimizes rounding errors caused when 


rounding non—-integer coordinate values to integer values for matrix cell 


x-—coordinate of origin of distribution circle 





% xinit=190; % For testing purposes only 























yinit=H—3*L/2; % y-coordinate of origin of distribution circle 
& yinit=80; % For testing purposes only 

psiinitdeg = 0; % Initial Heading in degrees 

psiinit = pi/180*«psiinitdeg;% Initial Heading in radians 

Sx*x*x*xFINAL PARAMETER VALUES! !!««x«x 

xfin=3; 

yfin=5; 

psi_d=—45; % Final yaw angle in degrees 


psifin=psi_d*pi/180; 


oo 
OO 


ol? 


% will most likely be necessary to generat 


fe) 





ol? 


Heuristic Boundaries 











Final yaw angle 


in radians 





Note: HB are completely artificially generated here. In the future, it 





& initial sonar sweeps. 





if ProbFlag 


end 


global BB UB RB LB RBx LBx UBy BBy 





% Upper Heuristic Bound (top wall) 
UB=R; 

% Lower (Bottom) Heuristic Bound 
BBy=yinit—L/2; 

BB=H+R-BBy; 


& Right Heuristic Bound (Right wall) 


RB=W+R; 


% Left Heuristic Bound 
LBx=xinit—L/2; 
LB=LBx+R; 





these on the fly based on 


% Bounding box corners for plotting purposes 


UBy=H+R-UB; 

RBxX=RB—R; 

xHCorners=[LBx LBx RBx RBx LBx]; 
yHCorners=[BBy UBy UBy BBy BBy]; 





fe: 


116 


117 


118 


119 


120 


121 


122 


123 


124 


125 


126 


127 


128 


129 


130 


131 


132 


133 


134 


135 


136 


137 


138 


139 


140 


141 


142 


143 


144 


145 


146 


147 


148 


149 


150 


151 


152 


153 


154 


155 


156 


2° 
C6 


if ProbFlag 


SensorModelGeneration; 


end 


$% Object 


Initialize Sensor model 


yu xl2 xr2 yl2 yu2 








Left boundary of object 1 
Right boundary of object 1 





Lower boundary of object 1 


global xl xr yl 
% Object 1 
xl=6; % 
xr=7; % 
yl=7; % 
yu=8; % 


% Object 2 


x12=8; 
xr2=9; 
yi 








ECT METHODS OPTIMIZATION 


Upper boundary of object 1 


Left boundary of object 2 
Right boundary of object 2 





Lower boundary of object 2 


Upper boundary of object 2 


(IDVD) 


BR KKK KKK KKK KR KKK KKK KK RK KR KKK KKK KR KK RK KK KK KKK KR KKK KKK KKK KR KKK KKK KEK KR KKK KKK KKK K 


ol? 





Everthing below this is part of the DM process. 


BR KKK KKK KKK RK KK KKK KK KKK KKK KKK KR KKK KKK KKK KR KKK KKK KKK KKK KR KKK KKK KKK RK KKK KKK KKK 


Fine_v Cost_T Fine _YawRate 














Defining optimization problem 


wl wY wu wv wb 


global 

global 

wl = led; 
wY = 1e2; 
wu = le0; 
wy = 1e3; 
wb = 1le0; 


if ProbFlag 
global wG wH wA wE Cost_EV Cost_H Cost_IG 


wG 
wH 
wA 
wE 


lel; 
1le0; 
le0; 
lel; 


ol? 


weighting 


ol? 


weighting 


ol? 


weighting 


ol? 


weighting 





ol? 


weighting 


\ 


ol? 





\° 
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+ weighting coeffici 


weighting coeffici 


coefficient 
coefficient 
coefficient 
coefficient 


coefficient 


weighting coefficient 


Lent 





ent 


+ weighting coefficient 


Fine_u Fine_beta 


EO 
iO 
For 
LO 


for 


time of arrival 

yaw rate 

surge velocity u 
sway velocity v 

beta 


Cost_AP avrad 


ahs 
FOr 
for 


i Oe 


Information Gain 





Heuristics Violations 





allision avoidance 


unexplored region penalty 


157 


158 


159 


160 


161 


162 


163 


164 


165 


166 


167 


168 


169 


170 


171 


172 


173 


174 


175 


176 


177 


178 


179 


180 


181 


182 


183 


184 


185 


186 


187 


188 


189 


190 


191 


192 


193 


194 


195 


196 


197 


avrad=L; % 














Obstacle Allision Avoidance 


Setting the boundary conditions 


% initial position 


% final position 


global vxi posxi vxf posxf 

global vyi posyi vyf posyf lami lamf 
posxi = xinit; posyi = yinit; 

posxf = xfin; posyf = yfin; 

vxi = 0; vyi = 0; % 
vxf = 0; vyf = 0; % 

% accxi = 0; accyi = 0; Q 
% accxf = 0; accyf = 0; g 





% betai=atan2 (vxi,vyi)—psiinit; 


% components of final 


% components of initial 


radius 


components of initial velocity 


components of final velocity 





acceleration 


acceleration 





betaf=atan2 (vxf, vyf)—psifin; 









































psii=psiinit; & psif=psifin; % initial and final yaw angles 
lami=1; lamf=1; %$ initial and final lambdas 
6% Guessing on the varied parameters 

guess (1)=15; % virtual arc length (tauf) 

guess (2)=0.5; % magnitude of final accel (accxf) 
guess (3)=—Ox«pi/4; % direction of final accel (accyf) 
guess (4) =0.25; & magnitude of initial accel (accxf) 
guess (5)=3*pi/4; % direction of initial accel (accyf) 
guess (6)=—.1; % initial psi double prime (psippi) 
guess (7)=—0.054; % final psi double prime (psippf) 
guess (8)=.0017; % initial lambda—dbl—prime 

guess (9)=.0009; % fina ambda—db1l—prime 

guess (10)=—pi/2; & final psi 





20 
OO 


Define the Reference 


RefFuncs; SNote: 


Svectorss in the trajectory function causes an error. 


Sit this way for now. 


Function Coefficients 


Doing this the other way by including the coefficient 


Keeping 





However, 


some computational 


optimization 


Smay be acheived by including the output of this function 


$directly into the trajectory.m script to eliminate the 





Ssymbo 








ic math computations performed in this function. 


The 


scoefficient vectors have been left in trajectory.m but 


scommented out to allow for future implementation. 


If the 


Sother method is used, this line may be commented out. 
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6% Calling the optimization routine 





maxiter=1000; %$ maximum number of iterations within fminsearch 
maxfuns=5*maxiter; $ maximum number of function evaluations 


fe) 


& Setting the options for fminsearch: 
opt=optimset ('Display','iter', 'TolX',1le—2,'TolFun',1le—-2,... 
"MaxIter',maxiter, 'MaxFunEvals',maxfuns); 


t = cputime; & Used to calculate and display actual computational time 


eee ee ee ee a ee ee ee ee ee ee ee eee 
% Calling the fiminsearch optimization function: 

[guess_opt,fval,exitflag] = fminsearch('trajectory',guess,opt) ; 

ee ee ee ee ee ee ee ee ee ee ee ee ee ee ee ee ee ee es 
fo} 


& calculating and displaying computational time: 


time_elapsed = cputime-t; 





fprintf('fminsearch Elapsed Time: %4.2g minutes.\n\n',time_elapsed/60) 
profile off 


6% Displaying cost functions and penalties 


if ProbFlag 





fprintf(' Information Gain Cost function : %6.2g\n',Cost_IG) 
fprintf(' Heuristics Violation Cost function : %6.2g\n',Cost_H) 
fprintf(' Allision Avoidance Cost function : %6.2g\n',Cost_AP) 
fprintf(' Unexplored Territory Cost function : %6.2g\n',Cost_EV) 

end 

fprintf(' Time Cost function : $6.29\n",Cost_T) 

fprintf(' Penalty in sway v : %6.2g\n',Fine_v) 

fprintf(' Penalty in surge u : %6.2g\n',Fine_u) 

fprintf(' Penalty in YawRate : $6.2g\n',Fine_YawRate) 

fprintf(' Penalty in beta ‘ $6.2g\n\n',Fine_beta) 


6% Displaying optimal parameters 


fo) 


% Display initial guesses on varied parameters 





disp('Initial Guesses on Values of Varied Parameters:') 

















fprintf('Arc length = %6.2f\n',guess(1)) 

fprintf('Final x,y—accel magnitude = %6.2f\n"',guess(2)) 
fprintf('Final x.y—accel direction = %6.2f deg\n', guess (3) *180/pi) 
fprintf('Initial x,y—accel magnitude = %6.2f\n',guess (4) ) 
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fprintf('Initial x, y—accel direction = %6.2f deg\n',guess(5)+*180/pi) 
fprintf('Initial psi accel = %6.2f\n"',guess(6)) 

fprintf('Final psi accel = %6.2f\n',guess (7) ) 

fprintf('Initial lambda dbl prime = %6.2f\n"',guess (8) ) 
fprintf('Final lambda dbl prime = %6.2f\n',guess(9)) 

fprintf('Final psi = %6.2f£\n\n',guess(10)+*180/pi) 











fe) 


% Display final values of varied parameters 


disp('Final Values of Varied Parameters:') 
































fprintf('Arc length = %6.2f\n',guess_opt (1)) 

fprintf('Final x, y—accel magnitude = %6.2f\n',guess_opt (2)) 
fprintf('Final x.y—accel direction = %6.2f deg\n',guess_opt (3) *«180/pi) 
fprintf('Initial x, y—accel magnitude = %6.2f\n',guess_opt (4) ) 
fprintf('Initial x, y—accel direction = %6.2f deg\n',guess_opt (5) *180/pi) 
fprintf('Initial psi accel = %6.2f\n',guess_opt (6) ) 

fprintf('Final psi accel = %6.2f\n',guess_opt(7)) 

fprintf('Initial lambda dbl prime = %6.2f\n',guess_opt (8) ) 
fprintf('Final lambda dbl prime = %6.2f\n',guess_opt (9)) 

fprintf('Final psi = %6.2f\n\n',guess_opt (10) *180/pi) 


6% Plotting the results 
PlotResults 


profile off 


% The following line saves all variables in the workspace to a .mat file 





% for future reference. Change this each run to avoid overwriting 


% previous saves 





ol? 


save ('Run_3—22_1_ incwv_facingN—fminsearch—1000iter') 





A.2. Trajectory Generation and Optimization Function: trajectory.m 


function PI=trajectory (guess) 


6% This function computes states and controls for the current guess. 





ol? 


It is the input function for fminsearch, and cannot be run alone. It is 


ol? 


the function to be optimized by fminsearch. 
lobal VARS ProbFlag 





lobal vxi posxi vxf posxf lami lamf 

















fo) 


lobal vyi posyi vyf posyf psii % psif betai betaf 





Q Qa 
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46 


47 


global x y time V u v psi Psi tau lam beta psi_dot 


6% Initialize Matrices/Grids—see notes in script 


InitMatrices; 


6% Current values of varied parameters 


tauf = guess(1); 

accxf = guess (2)x*cos(guess(3)); 
accyf = guess(2)*sin(guess(3)); 
accxi = guess (4)*cos(guess(5)); 
accyi = guess (4)*sin(guess(5)); 





psippf = guess(7); 
lamppi = guess (8); 
lamppf = guess (9); 





( 
( 
( 
( 
psippi = guess(6); 
( 
( 
( 
(1 


psif guess (10); 


& virtual arc length 


% final x accel 





ol? 


final y accel 


S$ initial x accel 





% initial y accel 
% initial beta double prime 


& final beta double prime 





% initial lambda—dbl—prime 








& final lambda—dbl—prime 








oe 


final psi 


6% Defining parameters in M/NN nodes in the virtual domain 


global a apsi alam Mp Np Ol 


fe) 


% (x,y) Reference Function Coefficients 


% Note: Putting the coefficient matrices in here gives me an error, so I'm 





% keeping the RefFuncs subroutine for now. If the errors can be resolved, 


% it would be computationally qu 


% computations in RefFuncs and s 


% syms tf x0 xp0 xpp0O xf xpf xppf 


icker to eliminate the symbolic math 








imply use the coefficient vectors below. 


% a = [x0; 

% (—xpp0/3 — xppf/6)+«tf*2 — x0 + xf; 

% (t£*2*xpp0) /2; 

% —tf*2x« (xpp0/6 — xppf/6); 

% ((xppO + xppf) «t£*2)/(4*pi) + ((2*xp0 — 2*xpf)*tf)/(4*pi); 


ol? 


(12*x0 — 12*xf)/(24*pi)]; 


% 


ax=subs (a, {'x0', 'xp0', 'xpp0', 'xf 


ay=subs (a, {'x0', 'xp0', 'xpp0', 'xf 


((xppO — xppf) «t£*%2)/ (24*pi) 


' 
Lf 


' 
i 


+ ((6exp0 + Gexpf) ett) / (24*epi) + 


Spt”, "spe EE hy eae 


{posxi,Vvxi,accxi,posxf,vxf,accxf,tauf}); 


TSE"; RPE! PIE}; wc 


{posyi,vyi,accyi,posyf,vyf,accyf,tauf}); 
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48 


49 


50 


S51 


52 


53 


54 


55 


56 


67 


68 


% (psi) Reference Function Coefficients 





% syms psi0 psipO psippO psiff psipf psippff tf 


% apsi=[psi0 
% psipOx*tf; 
% (psippO*«tf£*2) /2; 


ol? 


(psippff/2 — (3*psipp0)/2)«*«tf*2 + (—6*psipO — 4*psipf) «tf — 
10*psiO + 10*psiff; 


ae 


((3*psipp0)/2 — psippff)«tf*2 + (8*psipO + 7«*psipf)*tf + 
IS*xpsi0 — Ids4psitt; 
% (psippff/2 — psipp0/2)*«tf*2 + (—3*psipO — 3*psipf) «tf — 6*psi0 
+ 6*xpsiff]; 


ap=subs (apsi, {'psi0', 'psip0O', 'psipp0O', 'psiff', 'psipf','psippff','tf'},... 
{psii,0,psippi,psif,0,psippf,tauf}); 


% (lambda) Reference Function Coeffiecients 


% syms lam0O lampO lamppO lamff lampff lamppff tf 





% alam=[lam0; 

















% lampOx«tf; 

% (lamppO*t£*%2) /2; 

% (lamppff/2 — (3*lampp0)/2)*tf*2 + (— 6*lampO — 4*lampff) «tf — 
10*lamO + 10*lamff; 

% ((3*lampp0)/2 — lamppff)«tf*2 + (8*lampO + 7*xlampff)*tf + 
15*lamO — 15*lamff; 

% (lamppff/2 — lampp0/2)*tf*2 + (— 3*lampO — 3*lampff)*tf — 





6x*lamO + 6*lamff]; 


al=subs (alam, {'lam0', 'lamp0', 'lampp0O', 'lamff', 'lampff','lamppff','tf'},... 





{lami,0,lamppi,lamf,0,lamppf,tauf}); 


& If using the commented out coefficient vectors above, uncomment the 


% following lines as well: 





% Mp=length (a); 
& Nb=length(apsi); 
% Ol=length(alam); 


for i=1:Mp—2 
cx (i) =ax (Mp—1-i); 


cy (i) =ay (Mp—1-i); 
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end 


for i=1:Np 


cp (i) =ap (Np+1—i); 


end 


for i=1:01 
cl (i)=al (O1+1—-i); 





end 


2 


taubar=linspace(0,1); % taubar=tau/tauf (See report for why) 





& Evaluate complete ref funcs to obtain vectors of state parameters: 


x=polyval (cx, taubar) +ax(5)*sin(pi 








y=polyval(cy,taubar) tay (5) *sin(pi 











psi=polyval(cp,taubar) ; 











lam=polyval(cl,taubar); 





NN=length (x) ; 


%% Defining parameters' 


cx_prime=cx.*[3:—1:0]«eye(4,3); 


cy_prime=cy.*[3:—1:0]x«eye(4,3); 


ixtaubar) +ax (6) *sin(2*pixtaubar) ; 


ixtaubar) tay (6) *sin(2*pixtaubar) ; 


derivatives in NN nodes in the virtual domain 


X_prime=polyval (cx_prime,taubar) tax (5) *pixcos (pixtaubar) tax (6) *2*pix... 











cos (2*pixtaubar) ; 








y_prime=polyval (cy_prime,taubar) tay (5) *pixcos (pixtaubar) tay (6) *2*pix... 





x_prime=x_prime/tauf; 


y_prime=y_prime/tauf; 


cx_db] 


cos (2*pixtaubar) ; 


lprime=cx.*[6 2 0 Oj]xeye(4,2); 








cy_db] 


x_ 


y 


lprime=cy.*[6 2 0 Ojxeye(4,2); 

















_dblprime=polyval (cy_dblprime, taubar)—ay 


dblprime=polyval (cx_dblprime, taubar)—ax 


xpi®*2xsin(pixtaubar)—... 





) 
)* (2*pi) *2*sin(2*pixtaubar) ; 
)*pi®*2*sin(pixtaubar)—... 

) 


*(2*pi)*2*sin(2*pixtaubar) ; 





x_dblprime=x_dblprime/tauf%2; 











y_dblprime=y_dblprime/tauf%2; 


cp_prime=cp.*[5:—1:0]xeye(6,5); 


psi_prime=polyval (cp_prime,taubar) ; 
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psi_prime=psi_prime/tauf; 





cl_prime=cl.*[5:—1:0]xeye(6,5); 





lam_prime=polyval(cl_prime,taubar) ; 








lam_prime=lam_prime/tauf; 





6% Computing the states and controls using «Inverse Dynamics*« 


del_tau = tauf/ (NN-1); 
tau(1l) = 0; 
time(1) = 0; 





Psi = atan2(x_prime, y_prime); % 

Psi(1l) = Psi(2); Psi(end)=Psi(end—1); 
beta=Psi-—psi; 

V(1) = lam(1)*sqrt (x_prime (1) %*2+y_prime(1)%*2); 
u(1) = V(1) «cos (beta(1)); 

v(1) = V(1)*sin(beta(1)); 


Psi_prime(1l) = 


Computing heading, 


rad 


% Total speed 
% surge velocity 


% sway velocity 


(x_prime (1) *y_dblprime(1)—y_prime(1)*... 


x_dblprime (1))/(y_prime (1) *2+x_prime(1)%*2); 


Psi_dot (1) 
psi_dot(1) = 


lam(1)*Psi_prime (1); 





lam(1)*psi_prime (1); 


for j=2:NN 











tau(j) = tau(j—1)+del_tau; 

dt = 2«*del_tau/ (lam(j—1)+lam(j)); 
time(j) = time(j—1)+dt; 

V(j)= lam(j)*sqrt (x_prime(j)*2+y_prime(j)%2); 


u(3) = V(j)*cos(beta(j)); 
wt) = V(j)*sin(beta(j)); 


% Total speed 
% surge velocity 


fe) 


% sway velocity 


Psi_prime (j)=(x_prime (j) *«y_dblprime(j)—y_prime(j)*... 


x_dblprime(j))/(y_prime(j) *2+x_prime(j)%*2); 


Psi_dot (j)=lam(j)*Psi_prime (Jj); 
psi_dot(j) = lam(j)*psi_prime (4); 


end 


Psi_dot (end) =Psi_dot (end—1); 
beta_dot = Psi_dot — psi_dot; 
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184 
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186 


187 


188 


189 


190 


191 


192 


193 


194 


195 


196 


197 


198 


199 


200 


201 


202 


203 


204 


VARS=[lam' tau' time' x' y' u' v' V' x_prime'’ y_prime' beta' beta_dot'... 


psi' psi_dot' Psi' Psi_dot']; 


S%Sonar Imaging, Probabilistic Analysis, Occupancy Grid, Obstacle Avoidance 
if ProbFlag 





ee ee ee ee ee ee ee eee a a a aes 
2 


& Stuff below this is part of the probabilistic analysis process. 


ee ee ee ee ee ee ee ee ee ae 


global OG SnrImg PrOpoly PrEpoly 
global L IG UB BB RB LB AP RBx BBy ViolArea avrad 








$% Information Gain Analysis 





for i=1:NN 


° 


%& Perform simulated sonar sweep 





sweep (psi(i),x(i),y(i)); 


% Weight OG values at heuristic boundaries—this reduces the 
% information gain quantified by objects that we are already 
% assuming to be there. 

if i=l 

OG (10*UB, 10*LB:10*RB)=. 
OG (10*UB:10*BB, 10*RB)=. 





TBs 
Lae 
end 


end 


%% Update Occupancy Grid and Calculate Information Gain 


[IGC1]=OGupdate (SnriImg,PrOpoly,PrEpoly); % IGCl: IG Calculation 
IG=IGC1; 


6% Stern Points used for Heuristic Checks 





% Calculate the coordinates of the stern of the vehicle at each point 


% along the trajectory and determine if the stern points cross the 


% heuristics boundaries. 


for i=1:NN 


fo) 


xS(i)=x(i)-L/2*sin(psi(i)); % Calc x-coord of stern for 





each x_cg 
if xS(i) > RBx 
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228 
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230 


231 


232 


233 


234 


235 


236 


237 


238 


239 


240 


241 


242 


243 


244 


end 

yS(i)=y(i)-L/2*cos(psi(i)); % Calc y—coord of stern for 
each y_cg 

if yS(i) < BBy 
yViol (i)=1; 

élse 
yViol (i) =0; 

end 


end 


$% Heuristic Violation Area Analysis 





ol? 


Uses the vectors of stern point coordinates determined above to 


ole 





calculate the total violation area between the arc swept by the 





% vehicle stern and the heuristics boundaries. 





ViolArea=HeurViolArea(xS(:),yS(:),xViol(:),yViol(:)); 





$x*xx*x*xEND PROBABILISTIC (INFORMATION GAIN) ANALYSIS STUPFRkKkKKKKKKKKKKKKK 


SKK KKK KK RK KR KK KK KR KKK KKK KK KR KKK KKK KKK KKK KKK KKK KKK KKK RK KK KKK KK KKK KKK KK 


$% Allision Avoidance 








& Calculate the amount of penetration of an object/obstacle within a 





ole 


circle centered on the center of the vehicle with a give avoidance 


ale 


radius (defined in Main_opt.m script). 


AvoidancePenalty=zeros(NN,1); % Initialize vector (for speed) 


for i=1:NN 
AvoidancePenalty (i) =Avoidance (beta(i),x(i),y(i),avrad) ; 

end 

% Cumulative avoidance penalty along trajectory: 

AP=sum(AvoidancePenalty) ; 

6% Favoring Explored Areas 


% Calculate penalty for trajectorys that drive the vehicle outside of 








ole 


areas already swept by sonar (i.e. "explored areas"). This biases the 


ole 


resulting trajectories into known, or "explored" space. 
global EV 
for i=1:NN 
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255 
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266 


267 


268 


269 


270 


271 


272 


273 


274 


275 


276 


277 


278 


279 


280 


281 


282 


283 


284 


285 


fo) 


[me (i),ne(i) ]=xy2mn(x(i),y(i)); % Convert x,y coords to matrix coords 


OGcval=OG (round (10*me(i)),round(10*ne(i))); % Determine OG cell value 


fe) 


% at each x,y 











ev (1) =abs (OGcval—0O.5); $ Calculate absolute difference betw 
% OGVal and the OG value for an 
% unexplored cell (0.5). 

end 


EV=sum (ev) ; 
end 
6% Calculate Performance Index 
PI = PerformancelIndex; 


return 


function PI=PerformanceIndex 


ol? 
ale 


This function computes the combined performance index 


ol? 





ol? 


ol? 


for the Cost Function and Performance Index. The output is the value 


ol? 


that is actually being optimized within the fminsearch function by 


ol? 


changing the values of the variable parameters. 


Ty 


KKKKKKKKKKKKKKKKK KKK KKKKKKKKKK KKK KKK KKK KKKK KKK KKK KKK KKKKKKKKKKKKKKKKKKK KKK 


This function performs the actual calculations of the weighted penalties 


Bk KKK KKK KK KR KKK KKK KK KK KR KKK KKK KR KK KK KK KKK KK KR KKK KK KR KKK KKK KKK KEK KKK KKK KKK KKK 























global wu wv wb wY ProbFlag TH wT time Cost_T 
global u v u_max v_max psi_dot psi_dot_max 
global Fine_u Fine_v Fine_YawRate 
global Fine_beta beta beta_max 
Cost_T = (time (end)—TH) *%2; 
Fine _YawRate = max([0, (abs (psi_dot)—psi_dot_max) ])%2; 
Fine_u = max([0, (abs (u)—u_max) ]) *2; 
Fine_v = max([0, (abs (v)—v_max) ]) *2; 
Fine_beta = max([0, (abs (beta)—beta_max) ])%*2; 


if ProbFlag 
global Cost_IG Cost_H Cost_AP Cost_EV wG wH wA wE 
global ViolArea AP IG EV 














Cost_H = ViolArea; 
Cost_AP = AP; 
Cost_IG = 1/1G; 
Cost_EV = 1/(1+EV); 


end 
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if ProbFlag 
PI = wG*Cost_IGtwH*+Cost_H+wA*Cost_AP+wu+*+Fine_utwv*Fine_vtwYx... 








Fine_YawRate+wEx«Cost_EV+wT«Cost_T; 
else 
PI = wut*tFine_utwv*«Fine_vtwY*Fine_YawRatetwb+Fine betatwT*Cost_T; 


end 


return 


A.3. Reference Function Generation Function: RefFuncs.m 


ol? 


This script computes coefficients of the reference polynomials 
%% (x,y) Reference Function (scaled tau_bar=tau/tauf) 

global a apsi alam Mp Np Ol 

syms tf x0 xpO xpp0 xf xpf xppf 


A= [1000 0 0; 

0100 pi 2x*pi; 

0020 0 0; 

1111 £0 0; 

012 3 -—pi 2*pi; 

0026 0 Ol; 
b= [xO (xpO*tf) (xppO«tf*2) xf (xpf*tf) (xppf*tf*2)].'; 
a=A\b; 


a=collect(a,tf); 
Mp=length (a); 


%$% (psi) Reference Function (scaled tau_bar=tau/tauf) 





syms psi0 psipO psippO psiff psipf psippff tf 


c=[1 0 0 0 0 0; 
0 i 6 0 0 0; 
0 0 2 0 0 0; 
1 t | 1 + ay 
0 ti 2 3 4 5; 
0 0 2 6 12 20]; 


d=[psi0 psipOxtf psippO«*tf*2 psiff psipfxtf psippff«tf*2].'; 
apsi=C\d; 

apsi=collect (apsi,tf); 

Np=length(apsi); 


%$% (speed profile) Reference Function\ 
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30 


31 


32 


33 


34 


35 


36 


37 


38 


39 


40 


4l 





syms lamO lampO lamppO lamff lampff lamppff tf 
LL=[ 10 0 0 0 0; 


010000; 
002000; 
slimes as Ces Ue 
012345; 
002 6 12 20]; 


=[lamO lampO«xtf lamppOx«tf*2 lamff lampffxtf lamppff«tf*2].'; 
lam=LL\1; 














1 
a 
alam=collect (alam,tf); 
O 


l=length(alam); 


A.4 Sensor Model Generator Function: SensorModelGeneration.m 


% Sonar Sensor Model: 

% This function creates the probability density functions for the sonar 

% sensor model. It takes the pre-fit curve fits stored in this folder 

% (created using the cfit Matlab toolbox) and creates the polynomials 

% representing each sonar sensor probability model pdf for occupied/empty 
& states. The empirical data input in the sensor probabilities cells (ro 


%& and rE) represents the data used to create the pdf's and resulting curve 





& fits. This data is not actually used during runtime due to the fact that 
% it is already incorporated into the saved curve fits PrOmdlfunc and 
6 PrEmdlfunc. 


ae 


The pdf's may be plotted by uncommenting the plotting code in the last 
cell. 


ol? 


global PrOpoly PrEpoly 


fe) 


% Signal strengths (x-axis) 
SS=linspace(0,5000); 
SS=SS'; 


$% Sensor Probabilities (Occupied) 
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49 


53 


61 
63 
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. 6000; 
5000; 
5000; 
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26. 
28. 
5000; 
31. 


8000; 
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0000; 


6000; 
34. 
3:6 .< 
0000; 
40. 
42. 
44, 
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1000; 
3000; 
6000; 


.0000; 
ply 


5000; 


9500; 
56. 
58. 


3500; 
7000; 


.0000; 
-2500; 
65. 
67. 
69. 
- 7500; 
U3 
T% 
Wie 
Des 
-2500; 
83. 
84. 
3500; 
87. 
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6000; 
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7500; 
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6000; 
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0000; 
7000; 


9500; 


5000; 
9A: 
OF 
- 8500; 
.2000; 


0000; 
4500; 
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108 96.5000; 








109 97.7500; 
110 98.9500; 
1 100.1000; 
112 101.2000; 
113 102.2500; 
114 103225002 
115 104.2000; 
116 105.1000; 
17 105.9500; 
118 106.7500; 
119 107.5000; 
120 108.2000; 
121 108.8500; 
122 109.4500]; 
123 

14 % Normalize rO Vector (ensure sum(rO) = 1) 


ws rO=rO./sum(r0O); 

2 «© determine function representing this pdf 
ws load PrOmdlfunc; 

29 PrOpdf=PrOmdlfunc; 

30 6~PrOpoly=coeffvalues (PrOpdf) ; 

31 PrO=polyval (PrOpoly,SS); 





32 % S Plot pdf against original normalized sensor values 
33 3 PlLOE(SS, Pro," sb") 
B4 6 hold off 


& Plot error between rO and Pro 


i) 
a 
ol? 





37 & Err=PrO—rO; 


3s 6 plot (SS,Err) 





uo 6% Sensor Probabilities (Empty) 


12 % rE=normpdf (SS,500,500); 


144 TE=( 25734754; 62-673 71; 737747 74; 737713 65;57; 517457405357 31-27; 247215185167 2... 


145 Ae TA ei 10: 9°85 B85 7s 1s 65 67. 555357574 Ae AA Ae Se Se 37 3p OF OF OFF OF OF Op aes 
146 DD DDS Dies et Pn De Le De De ee en eae eo ec ales Meee eral eae deed et 1 cs ee sg cee 
147 ee oe ese pe thee se 1 i.e Le] 
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166 


167 


168 


169 


170 


171 


172 


fe) 





rE=rE./sum(rE) ; 


fe) 


load PrEmdlfunc2; 
PrEpdf=PrEmdlfunc2; 


PrEpoly=coeffvalues (PrEpdf) ; 


PrE=polyval (PrEpoly,SS); 


% PrE=PrE./sum(PreE); 


ao 9 
0 oO 








% Normalize rE Vector (ensure sum(rE) = 1) 


% determine function representing this pdf 


Plot pdf against original normalized sensor values 





% plot (SS, Pre, "ib") 
= hold off 





%% Plot both pdf's together 


% z=zeros(100,1); 


& figure () 


% p 








é . 
Oo xX 


abel ('Return Signal Value') 











6 yl 





Lot.iSs, Pre, ' tr" -SS,;PL0; '=—b" ,SSzzy “k") 


% title('Sonar Return Signal Probability Density Functions') 


abel('Probability of Receiving Signal Value") 


* legend('P[r_t_+_1 | s(C)=Emp]",*P[r_tu+_1 | s(C)=Oce]”") 


A.5 Sonar Sweep and Imaging Function: sweep.m 


function sweep( hdg,xv,yv_ ) 


> SWI 





% 
% 
% 
% 


% 


AP oP ol 


ol? 


BEEP Performs a sonar sweep simulation given known input mapping data 


This function takes vehicle state inputs and a known world model of the 








environment to determine signal strength values for occupied/unoccupied 


pixels in the sonar sweep. It then stores these signal strength values 


in a sonar image model matrix 


image to be used for updating 


Tnputs: 

hdg: Vehicle heading (psi) 
xy: X coordinate of vehicl 
yv: Y coordinate of vehicl 





representing a notional qualitative sonar 





the Occupancy Grid. 





in NED coordinate frame 


le centroid 








le centroid 


92 





46 


47 


48 


49 


50 


global SnriImg WorldMdl L H R Rs 





alpha = 45*pi/180; % Sonar spread half—width in radians 





SxxxxxNext four values are arbitrarily set. Once method is validated, this 





% might need to be changed so that the signal returns are more 
randomized 

% based on sonar sensor model.***xx 

sigOccMed = 4000; & Median signal strength to use for occupied cells 

sigOccDev = 750; % Max deviation from occupied median signal strength 

sigEmpMed = 300; % Median signal strength to use for empty cells 











oe 


sigEmpDev = 250; Max deviation from empty median signal strength 


SxxxxxxChange the next line to refine SWECPKKkaaeKaaKKKKKKAK 





fe) 


ang = 1*pi/180; % Angle increment in radians * 


ee ee ee ee ee ee ee ee ee ee ee ee ea ae 


xs = xv + L/2«sin(hdg); % Sonar transmitter x—coordinate in tank frame 
ys = yv + L/2x«cos (hdg) ; % Sonar transmitter y-—coordinate in tank frame 
% Note: following are scaled by 10 to change into the matrix space, vice 


& the cartesian space 
ms = round(10x* (R+(H-ys))); & Map matrix row index of ys 


ns = round(10x* (R+xs)); % Map matrix column index of xs 


ol? 


This 'for' loop iterates through sonar spread in angular increment 


ol? 


specified by variable 'ang'. At each step, it generates a Bresenham 


ol? 


line, which it then compares to the known image map ('WorldMdl') to 


assign 


ol? 


a value of 1 or 0 to each point on the Bresenham line corresponding to 


oe 


the value for that point within the image map. 


for a=hdg—alpha:ang:hdg+talpha 


mr=ms—round(Rsx«cos (a)); % row index of Bresenham line endpoint 
y—coord 

nr=nstround(Rs*sin(a)); % column index of Bresenham line endpoint 
x-—coord 


° 


% Bresenham line creation 
[bn bm] = bresenham(ns,ms,nr,mr) ; 
len = length(bn); 
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55 


56 


57 


58 


59 


60 


61 


62 


63 


64 


65 


66 


67 


68 


69 


70 


71 


72 


73 


74 


75 


76 


end 


end 


bv=zeros(1,len); 


for j=l:len 


bv (3) =WorldMdl (bm(j),bn(j)); 


point on 


% Assign 1 or 0 value to each 


% Bresenham line corresponding 


to those 


fe) 


% points in input binary matrix 


& This next part chooses random values between min empty/occupied 


% signal and max empty/occupied signal as defined above. This 


% randomizes the signal return value, adding realism to the 





ol? 


if brig 


simulation. 


) == 


sigEmpmin=sigI 








sigl 





Empmax=Ssig] 





sigEmp=randi([sigEmpmin sig 


EmpMed—sigEmpDev; 
EmpMed+sigEmpDev; 





Empmax]) ; 





SnriImg(bm(j),bn(j)) = sigEmp; 


elseif bv(j) == 


sigOccmin=sigOccMed—sigOccDev; 


sigOccmax=sigOccMedt+sigOccDev; 


sigOcc=randi([sigOccmin sigOccmax]) ; 





Snrimg(bm(j),bn(j)) = sigOcc; 


break 


end 


end 


A.6 Occupancy Grid Updater and Information Gain Calcu- 


lation Function: OGupdate.m 


function [IGl ]=OGupdate ( 








snrimg, occpdf 


,emppdf ) 


SOGUPDATE Updates the probabilities in Occupancy Grid for each sonar sweep 


Sand cal 


2 
Oo 
° 
oO 
° 
oO 


° 
oO 


lculates the resulting information gain. 





This function takes the sonar image developed in the sweep function as 





well as the sensor signal probability density functions, and 


updates the Occupancy Grid usign the Bayesian Inference equation. It 


then calculates the information gain acheived during each cell update, 


94 





42 


45 


46 


ol? 


% occupancy grid for the given trajectory. 


global OG SHE 





[rx c]=size(snrimg) ; 


igl=zeros(r,c); % Matrix preallocation for speed 
%& ig2=zeros(r,c); 
for a=12xr 


for j=lic 





if snrimg(i,j) 0 
PsO = polyval(occpdf,snrimg(i,j)); 





PsE = polyval(emppdf, snrimg(i,j)); 


elseif snrimg(i,j) == 


PsE=0.5; 
PsO=0.5; 
end 
PcOt=O0G (i,j); % Prior OG cell value 


fe) 


% Bayesian inference formula for posterior OG cell value: 
PcO=(PsOxPcOt) / (PSE* (1—PcOt) +PSO*PcOt) ; 


% Simple IG method 
igl (i, j)=abs (PcO—PcOt) ; 


ol? 


Alternate IG method: This method was derived from the 


ol? 


ol? 





ol? 


ol? 


at this time. 


ol? 





Update Entropy 


oe 


Pi=OG(i,j); % Prior cell occupancy probability 


ole 


cell is Occupied 


oe 


entropy 


oe 


HE (i, j)=Hp; % Update entropy grid 


ole 


ole 


Eqn. 17.15, pg. 584) 


95 


and finally calculates the cumulative information gain over the 


Probabilistic Robotics book. However, it proved to be very 


Ptrue=PcO; % Probability for correctly measuring 


Hp=—Pi+log2 (Pi)—(1—Pi) *log2 (1—Pi); % Prior cell 


entire 


computationally intensive, and produced results very close to the 


much simpler method actually employed above, thus it is not used 


Expected entropy after sensing (Probabilistic Robotics, 


47 


48 


49 


50 


51 


52 


33: 


54 


55 


56 


57 


58 


59 


60 


61 


62 


63 





ol? 





EH=—Pt rue*Pixlog2 (Ptruex*Pi/ (Ptrue*Pi+(1—Ptrue)*(1—Pi)))... 


ae 








(1—Ptrue) * (1—Pi) *log2 ((1—Ptrue) « (1—Pi) / (Ptrue*Pit+(1—Ptrue)*(1—Pi)))... 


oe 





—(1—Ptrue) *Pixlog2((1—Ptrue) *«Pi/ (Ptrue* (1—Pi)+(1—Ptrue)+*Pi))... 





% —Ptruex (1—Pi) *log2 ((1—Ptrue) *Pi/ (Ptruex (1—Pi)+(1—Ptrue) *Pi)); 
% ig2(i,j)=Hp—-EH; % Expected Information Gain (Probabilistic 


Robotics, 








ol? 

oe 

ica) 
fe} 


n. 17.4, pg. 572) 


fe) 


% Update Occupancy Grid 
OG (i, 3) =Pc0; 


end 
end 
TGl=sum(sum(igl)); % Calculate total IG over entire OG 


& IG2=sum(sum(ig2) ); 


A.7 Heuristics Violation Analysis Function: HeurViolArea.m 


function [ ViolArea ] = HeurViolArea( xS,yS,xViol,yViol ) 





oe 











HEURVIOLAREA—calculates heuristics boundary violation areas 


ole 


This function uses the vectors of stern point coordinates to 


ae 





calculate the total violation area between the arc swept by the 





% vehicle stern and the heuristics boundaries. 


global RBx BBy 


if (any(xViol)) && (any(yViol)) 
ViolArea=NaN; 
elseif (any(xViol)) || (any(yViol) ) 


if any (xViol) 

% fprintf('Run Sg: Violation of Right Boundary.\n',n) 
xViolVec=xS (find(xViol)); 

xvl=length (xViolVec) ; 

















for i=l:xvl 


96 


xV(1i)=xViolVec (i)—RBx; 


end 


fo) 


% **x*xCalculate Violation Area 





xVint=trapz 


(yS (find(xViol)),xV); 


ViolArea=abs (xVint); 


% fprintf('Violation of Right Boundary: %6.4g\n',ViolArea) 


KKK 


ol? 


%& *xxPlot right boundary violation area—use this for a visual plot 





% of the actual boundary violations. 


RBxp=ones 


(Lp xvl)seRBS 


xviolfig=figure(); 


plot (yS (£1 


nd(xVi0l)),xVielVec, "hk", ys (find (xVi0l) ),RBxp, re")? 








jbfillt(ys 





(find(xViol)),RBxp, xViolVec) ; 





axis equal 





tstr=sprintf('Run %g Right Boundary Violation Plot',n); 
title (tstr) 





xlabel('y') 











ylabel('x") 


legend('Stern Path','Right Boundary', 'Location', 'North') 


set (gca,'YDir', 'reverse') 
camroll (90) 


Co KKK 


end 


if any (yViol) 


fo) 


yViolVec=ys 





% fprintf('Run Sg: Violation of Bottom Boundary. \n',n) 


(find(yViol)); 


yvl=length (yViolVec) ; 


fe) 


&[myv nyv]=size(yViolVec) % t/s only 


for i=l:yvl 


yV (i) =BBy—yViolVec (i); 


end 


fo) 


% ***xCalculate Violation Area 





yVint=trapz 


(xS (find(yViol)),yV); 


ViolArea=abs (yVint) ; 


fo) 


BS ke* 


& fprintf('Violation of Lower Boundary: %6.4g\n',ViolArea) 
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fe) 


% *xxPlot bottom boundary violation area—use this for a visual 
plot 


% of the actual boundary violations. 


ol? 


BByp=ones (1, yvl) *BBy; 


ol? 


[mby nby]=size(BByp) %t/s only 


ol? 


yviolfig=figure(); 
plot.(xS(£ind(yViol)) ,yVvielVec, 'k";<S.(fand (yVLol),),BBYp; * re") 
jbfill(xS (find (yViol)),BByp, yViolVec) ; 


ae ol 


ol? 


axis equal 


ol? 


tstr=sprintf('Run %g Bottom Boundary Violation Plot',n); 














% title (tstr) 
% xlabel('x') 
% ylabel('y') 
% legend('Stern Path','Bottom Boundary', 'Location', 'Best') 
EE ewe 
end 
else ViolArea = 0; 
end 
end 


A.8 Avoidance Penalty Analysis Function: Avoidance.m 


ol? 


%& for the sonar image, heuristics analysis, occupancy grid, and information 


ae 


gain value storage. 
% World Model (Input data for simulation) 

global HMat OG SnriImg WorldMdl RMN IGl1 ObsFlag Rs H xl xr yl yu x12 
xr2 yl2 yu2 %HE 








fe) 


6 Tank matrix for Bresenham imaging algorithm 





Rs=10+*R; %& Scales the sonar range value for matrix cell 
% index addressing. 
WorldMdl=zeros (M,N); 


% The following 4 'for' loops place ones in tank matrix space outside 
the tank 
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This script generates the World Model matrix and initializes the matrices 





for i=1:Rs 
WorldMdl (i,:)=1; 
end 
for jJ=M-Rs:M 
WorldMdl (j,:)=1; 
end 
for k=1:Rs 
WorldMdl (:,k)=1; 
end 
for 1=N-Rs:N 
WorldMdl(:,1)=1; 


end 


if ObsFlag 

% Obstacles in tank: 
m1l=10* (R+H—-yu) ; 
m2=10* (R+H—-yl); 
nl=10* (x1+R); 

n2=10* (xr+R); 


WorldMdl (m1l:m2,n1:n2)= 


m12=10x* (R+H—yu2) ; 
m22=10* (R+H-yl2) ; 
n12=10x* (x12+R); 

n22=10x* (xr2+R); 





Lee 3% 


Assigns value of one to all World Model 





cells corresponding to the obstacle location. 


WorldMdl (m12:m22,n12:n22)=1; 


end 


oe 


SnriImg=zeros (M,N); 
HMat=WorldMdl; 
OG=.5*ones (M,N) ; 
IGl=zeros (M,N) ; 


el 


ae 


Ini 


tial 





Ini 


tial 


ize Sonar Image matrix 


Used for Heuristic Violation Analysis 








Ini 





tial 


ize Occupancy Grid 


ize Information Gain matrix 
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